Skip to content

Commit

Permalink
Gvf for rotorcrafts (#3197)
Browse files Browse the repository at this point in the history 
* Core GVF changes
* Update GVF module conf file for rotorcrafts
* Integration of acceleration in guidance rotorcraft firmware
* Update command chain in guidance for ACCEL command integration
* [guidance] select guidance sp mode with enum instead of bitmask
* [guidance] reset ref if input type has changed

---------

Co-authored-by: Mael FEURGARD <mael.feurgard@laas.fr>
Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>
  • Loading branch information
@spacewolfXfr @gautierhattenberger
3 people committed Dec 19, 2023
1 parent cf23b6b commit 6614729
Show file tree
Hide file tree
Showing 9 changed files with 232 additions and 69 deletions.
9 changes: 8 additions & 1 deletion conf/modules/gvf_module.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,14 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
<file name="trajectories/gvf_ellipse.c"/>
<file name="nav/nav_survey_polygon_gvf.c"/>
</makefile>

<makefile firmware="rotorcraft">
<file name="gvf.c"/>
<file name="gvf_low_level_control.c"/>
<file name="trajectories/gvf_line.c"/>
<file name="trajectories/gvf_sin.c"/>
<file name="trajectories/gvf_ellipse.c"/>
</makefile>

<makefile firmware="rover">
<file name="gvf.c"/>
Expand All @@ -94,4 +102,3 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
</makefile>

</module>

87 changes: 67 additions & 20 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ void guidance_h_init(void)
FLOAT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0.0;
guidance_h.sp.heading_rate = 0.0;
guidance_h.sp.h_mask = GUIDANCE_H_SP_POS;
guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW;
transition_percentage = 0;
transition_theta_offset = 0;

Expand Down Expand Up @@ -252,7 +254,7 @@ void guidance_h_read_rc(bool in_flight)
#if GUIDANCE_H_USE_SPEED_REF
read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight);
/* enable x,y velocity setpoints */
SetBit(guidance_h.sp.mask, 5);
guidance_h.sp.h_mask = GUIDANCE_H_SP_SPEED;
#endif
break;

Expand Down Expand Up @@ -345,7 +347,13 @@ 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)) {
if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) {
struct FloatVect2 sp_accel;
sp_accel.x = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.x);
sp_accel.y = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.y);
gh_update_ref_from_accel_sp(sp_accel);
}
else if (guidance_h.sp.h_mask == 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);
Expand All @@ -364,18 +372,32 @@ static void guidance_h_update_reference(void)
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 {
if (nav.setpoint_mode == NAV_SETPOINT_MODE_POS) {
VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel);
} else { //(nav.setpoint_mode == NAV_SETPOINT_MODE_SPEED)
guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
guidance_h.ref.speed.x = guidance_h.sp.speed.x;
guidance_h.ref.speed.y = guidance_h.sp.speed.y;
guidance_h.ref.accel.x = 0;
guidance_h.ref.accel.y = 0;
} // TODO: make accel ref set
switch (nav.setpoint_mode) {
case NAV_SETPOINT_MODE_SPEED:
guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
guidance_h.ref.speed.x = guidance_h.sp.speed.x;
guidance_h.ref.speed.y = guidance_h.sp.speed.y;
guidance_h.ref.accel.x = 0;
guidance_h.ref.accel.y = 0;
break;

case NAV_SETPOINT_MODE_ACCEL:
guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
guidance_h.ref.speed.x = stateGetSpeedNed_i()->x;
guidance_h.ref.speed.y = stateGetSpeedNed_i()->y;
guidance_h.ref.accel.x = guidance_h.sp.accel.x;
guidance_h.ref.accel.y = guidance_h.sp.accel.y;
break;

case NAV_SETPOINT_MODE_POS:
default: // Fallback is guidance by pos
VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel);
break;
}
}

#if GUIDANCE_H_USE_SPEED_REF
Expand All @@ -385,7 +407,7 @@ static void guidance_h_update_reference(void)
#endif

/* update heading setpoint from rate */
if (bit_is_set(guidance_h.sp.mask, 7)) {
if (guidance_h.sp.yaw_mask == GUIDANCE_H_SP_YAW_RATE) {
guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY;
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
}
Expand Down Expand Up @@ -478,7 +500,8 @@ void guidance_h_from_nav(bool in_flight)
break;

case NAV_SETPOINT_MODE_ACCEL:
// TODO set_accel ref
guidance_h_set_acc(nav.accel.y, nav.accel.x); // nav acc is in ENU frame, convert to NED
guidance_h_update_reference();
guidance_h_set_heading(nav.heading);
guidance_h_cmd = guidance_h_run_accel(in_flight, &guidance_h);
break;
Expand Down Expand Up @@ -559,14 +582,17 @@ void guidance_h_guided_run(bool in_flight)

void guidance_h_set_pos(float x, float y)
{
ClearBit(guidance_h.sp.mask, 5);
if (guidance_h.sp.h_mask != GUIDANCE_H_SP_POS) {
reset_guidance_reference_from_current_position();
}
guidance_h.sp.h_mask = GUIDANCE_H_SP_POS;
guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
}

void guidance_h_set_heading(float heading)
{
ClearBit(guidance_h.sp.mask, 7);
guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW;
guidance_h.sp.heading = heading;
FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading);
}
Expand All @@ -581,14 +607,35 @@ void guidance_h_set_body_vel(float vx, float vy)

void guidance_h_set_vel(float vx, float vy)
{
SetBit(guidance_h.sp.mask, 5);
if (guidance_h.sp.h_mask != GUIDANCE_H_SP_SPEED) {
reset_guidance_reference_from_current_position();
}
guidance_h.sp.h_mask = GUIDANCE_H_SP_SPEED;
guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
}

void guidance_h_set_body_acc(float ax, float ay)
{
float psi = stateGetNedToBodyEulers_f()->psi;
float newax = cosf(-psi) * ax + sinf(-psi) * ay;
float neway = -sinf(-psi) * ax + cosf(-psi) * ay;
guidance_h_set_acc(newax, neway);
}

void guidance_h_set_acc(float ax, float ay)
{
if (guidance_h.sp.h_mask != GUIDANCE_H_SP_ACCEL) {
reset_guidance_reference_from_current_position();
}
guidance_h.sp.h_mask = GUIDANCE_H_SP_ACCEL;
guidance_h.sp.accel.x = ACCEL_BFP_OF_REAL(ax);
guidance_h.sp.accel.y = ACCEL_BFP_OF_REAL(ay);
}

void guidance_h_set_heading_rate(float rate)
{
SetBit(guidance_h.sp.mask, 7);
guidance_h.sp.yaw_mask = GUIDANCE_H_SP_YAW_RATE;
guidance_h.sp.heading_rate = rate;
}

25 changes: 24 additions & 1 deletion sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,9 +79,20 @@ struct HorizontalGuidanceSetpoint {
*/
struct Int32Vect2 pos;
struct Int32Vect2 speed; ///< only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
struct Int32Vect2 accel; ///< For direct control of acceleration, if the guidance scheme is able to provide it

float heading;
float heading_rate;
uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw

enum {
GUIDANCE_H_SP_POS = 0,
GUIDANCE_H_SP_SPEED = 1,
GUIDANCE_H_SP_ACCEL = 2
} h_mask;
enum {
GUIDANCE_H_SP_YAW = 0,
GUIDANCE_H_SP_YAW_RATE = 1
} yaw_mask;
};

struct HorizontalGuidanceReference {
Expand Down Expand Up @@ -146,6 +157,18 @@ extern void guidance_h_set_body_vel(float vx, float vy);
* @param vx North velocity (local NED frame) in meters/sec.
* @param vy East velocity (local NED frame) in meters/sec.
*/
extern void guidance_h_set_acc(float ax, float ay);

/** Set body relative horizontal acceleration setpoint.
* @param vx forward acceleration (body frame) in meters/sec².
* @param vy right acceleration (body frame) in meters/sec².
*/
extern void guidance_h_set_body_acc(float ax, float ay);

/** Set horizontal acceleration setpoint.
* @param vx North acceleration (local NED frame) in meters/sec².
* @param vy East acceleration (local NED frame) in meters/sec².
*/
extern void guidance_h_set_vel(float vx, float vy);

/** Set heading rate setpoint.
Expand Down
20 changes: 20 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,26 @@ void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
VECT2_COPY(gh_ref.accel, accel_sp);
}

void gh_update_ref_from_accel_sp(struct FloatVect2 accel_sp)
{
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 = LBFP_OF_REAL(pos_step.x, GH_POS_REF_FRAC);
pos_update.y = LBFP_OF_REAL(pos_step.y, GH_POS_REF_FRAC);

VECT2_ADD(gh_ref.pos, pos_update);
VECT2_ADD(gh_ref.speed, speed_step);

gh_saturate_accel(&accel_sp);

// copy accel
VECT2_COPY(gh_ref.accel, accel_sp);
}

static void gh_saturate_speed(struct FloatVect2 *speed_sp)
{
// Speed squared
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ extern void gh_ref_init(void);
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_speed_sp(struct FloatVect2 speed_sp);
extern void gh_update_ref_from_accel_sp(struct FloatVect2 accel_sp);

/**
* Set a new maximum speed for waypoint navigation.
Expand Down
60 changes: 38 additions & 22 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -316,33 +316,49 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc
struct FloatVect3 pos_err = { 0 };
struct FloatVect3 accel_sp = { 0 };

if (h_mode == GUIDANCE_INDI_H_POS) {
pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
speed_sp.x = pos_err.x * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
speed_sp.y = pos_err.y * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
}
else if (h_mode == GUIDANCE_INDI_H_SPEED) {
speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
struct FloatVect3 speed_fb;


if (h_mode == GUIDANCE_INDI_H_ACCEL) {
// Speed feedback is included in the guidance when running in ACCEL mode
speed_fb.x = 0.;
speed_fb.y = 0.;
}
else { // H_ACCEL
speed_sp.x = 0.f;
speed_sp.y = 0.f;
else {
// Generate speed feedback for acceleration, as it is estimated
if (h_mode == GUIDANCE_INDI_H_SPEED) {
speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
}
else { // H_POS
pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
speed_sp.x = pos_err.x * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
speed_sp.y = pos_err.y * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
}
speed_fb.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
speed_fb.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
}

if (v_mode == GUIDANCE_INDI_V_POS) {
pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
speed_sp.z = pos_err.z * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gv->zd_ref);
} else if (v_mode == GUIDANCE_INDI_V_SPEED) {
speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
} else { // V_ACCEL
speed_sp.z = 0.f;
if (v_mode == GUIDANCE_INDI_V_ACCEL) {
// Speed feedback is included in the guidance when running in ACCEL mode
speed_fb.z = 0;
}
else {
// Generate speed feedback for acceleration, as it is estimated
if (v_mode == GUIDANCE_INDI_V_SPEED) {
speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
}
else { // V_POS
pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
speed_sp.z = pos_err.z * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gv->zd_ref);
}
speed_fb.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
}

accel_sp.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
accel_sp.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
accel_sp.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
accel_sp.x = speed_fb.x + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
accel_sp.y = speed_fb.y + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
accel_sp.z = speed_fb.z + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);

return guidance_indi_run(&accel_sp, gh->sp.heading);
}
Expand Down

0 comments on commit 6614729

Please sign in to comment.