Skip to content

Commit

Permalink
[rotorcraft] guidance_h speed ref saturation fix
Browse files Browse the repository at this point in the history 
Bound ref to max accel along reference vector.
If angle can't be computed, simply set both axes to max magnitude instead of zero.

Should (tm) fix #716
  • Loading branch information
@flixr
flixr committed May 13, 2014
1 parent 754a8c8 commit 9bcccc6
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 35 deletions.
4 changes: 2 additions & 2 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -399,7 +399,7 @@ static void guidance_h_update_reference(void) {
}

#if GUIDANCE_H_USE_SPEED_REF
if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) {
if (guidance_h_mode == GUIDANCE_H_MODE_HOVER) {
VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only
}
#endif
Expand Down Expand Up @@ -513,7 +513,7 @@ static inline void transition_run(void) {

/// read speed setpoint from RC
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) {
if(in_flight) {
if (in_flight) {
// negative pitch is forward
int64_t rc_x = -radio_control.values[RADIO_PITCH];
int64_t rc_y = radio_control.values[RADIO_ROLL];
Expand Down
62 changes: 29 additions & 33 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ struct Int32Vect2 gh_speed_ref;
struct Int64Vect2 gh_pos_ref;


static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC);
static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC);

float gh_max_speed = GUIDANCE_H_REF_MAX_SPEED;

Expand Down Expand Up @@ -174,12 +174,14 @@ static void gh_compute_route_ref(struct Int32Vect2* ref_vector) {
}

static void gh_compute_ref_max(struct Int32Vect2* ref_vector) {
/* Compute route reference before saturation */
/* Bound ref to max speed/accel along route reference angle.
* If angle can't be computed, simply set both axes to max magnitude.
*/
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_max_accel_ref.x = 0;
gh_max_accel_ref.y = 0;
gh_max_speed_ref.x = 0;
gh_max_speed_ref.y = 0;
gh_max_accel_ref.x = gh_max_accel;
gh_max_accel_ref.y = gh_max_accel;
gh_max_speed_ref.x = gh_max_speed_int;
gh_max_speed_ref.y = gh_max_speed_int;
}
else {
gh_compute_route_ref(ref_vector);
Expand All @@ -189,16 +191,18 @@ static void gh_compute_ref_max(struct Int32Vect2* ref_vector) {
/* Compute maximum reference x/y velocity from absolute max_speed */
gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed_int, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed_int, 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));
}
/* 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));
}

static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) {
/* Compute route reference before saturation */
/* Bound ref to max accel along reference vector.
* If angle can't be computed, simply set both axes to max magnitude.
*/
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_max_accel_ref.x = 0;
gh_max_accel_ref.y = 0;
gh_max_accel_ref.x = gh_max_accel;
gh_max_accel_ref.y = gh_max_accel;
}
else {
gh_compute_route_ref(ref_vector);
Expand All @@ -209,56 +213,48 @@ static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) {
}

static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) {
/* Compute route reference before saturation */
/* Bound ref to max speed along reference vector.
* If angle can't be computed, simply set both axes to max magnitude.
*/
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_max_speed_ref.x = 0;
gh_max_speed_ref.y = 0;
gh_max_speed_ref.x = gh_max_speed_int;
gh_max_speed_ref.y = gh_max_speed_int;
}
else {
gh_compute_route_ref(ref_vector);
/* Compute maximum reference x/y velocity from absolute max_speed */
gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed_int, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed_int, 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));
}
/* 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 reference accelerations */
static void gh_saturate_ref_accel(void) {
/* 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;
}
BoundAbs(gh_accel_ref.x, gh_max_accel_ref.x);
BoundAbs(gh_accel_ref.y, gh_max_accel_ref.y);
}

/** Saturate ref speed and adjust acceleration accordingly */
static void gh_saturate_ref_speed(void) {
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
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;
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) {
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) {
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;
Expand Down

0 comments on commit 9bcccc6

Please sign in to comment.