From 27e59d9db5fffd9199fc963dbf2d8b586bd6717e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 21 Jul 2025 16:21:44 +0200 Subject: [PATCH] [guided] support full ref update in guided mode (#3502) New message can set position, speed, acceleration and heading setpoint for an accurate trajectory tracking. Also fix the update of ref when h_ref is disabled. --- conf/modules/autopilot_guided.xml | 1 + .../firmwares/rotorcraft/autopilot_guided.c | 26 ++++++ .../firmwares/rotorcraft/autopilot_guided.h | 4 + .../rotorcraft/guidance/guidance_h.c | 85 +++++++++++++------ .../rotorcraft/guidance/guidance_h.h | 25 ++++-- .../rotorcraft/guidance/guidance_v.c | 23 ++++- .../rotorcraft/guidance/guidance_v.h | 13 +++ sw/ext/pprzlink | 2 +- sw/lib/python/guided_mode.py | 18 ++++ 9 files changed, 162 insertions(+), 35 deletions(-) diff --git a/conf/modules/autopilot_guided.xml b/conf/modules/autopilot_guided.xml index 40703a281a..a8f6124960 100644 --- a/conf/modules/autopilot_guided.xml +++ b/conf/modules/autopilot_guided.xml @@ -15,6 +15,7 @@ + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c index 792bcbc982..f4a8143be5 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c @@ -163,6 +163,29 @@ void autopilot_guided_parse_GUIDED(uint8_t *buf) { DL_GUIDED_SETPOINT_NED_yaw(buf)); } +void autopilot_guided_parse_GUIDED_FULL(uint8_t *buf) { + if (DL_GUIDED_FULL_NED_ac_id(buf) != AC_ID || autopilot_get_mode() != AP_MODE_GUIDED) { + return; + } + + guidance_h_set_all( + DL_GUIDED_FULL_NED_x(buf), + DL_GUIDED_FULL_NED_y(buf), + DL_GUIDED_FULL_NED_vx(buf), + DL_GUIDED_FULL_NED_vy(buf), + DL_GUIDED_FULL_NED_ax(buf), + DL_GUIDED_FULL_NED_ay(buf) + ); + guidance_h_set_heading( + DL_GUIDED_FULL_NED_heading(buf) + ); + guidance_v_set_all( + DL_GUIDED_FULL_NED_z(buf), + DL_GUIDED_FULL_NED_vz(buf), + DL_GUIDED_FULL_NED_az(buf) + ); + +} #else bool autopilot_guided_goto_ned(float x, float y, float z, float heading) @@ -216,5 +239,8 @@ void autopilot_guided_parse_GUIDED(uint8_t *buf) { (void) buf; } +void autopilot_guided_parse_GUIDED_FULL(uint8_t *buf) { + (void) buf; +} #endif diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h index 777158c943..bb79662262 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h @@ -92,6 +92,10 @@ extern void autopilot_guided_update(uint8_t flags, float x, float y, float z, fl */ extern void autopilot_guided_parse_GUIDED(uint8_t *buf); +/** Parse GUIDED_FULL_NED messages from datalink + */ +extern void autopilot_guided_parse_GUIDED_FULL(uint8_t *buf); + /** Bitmask for setting the flags attribute in autopilot_guided_update function * See function description for more details */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 53d367cb0d..220aed4762 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -274,7 +274,16 @@ 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 (guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) { + if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ALL) { + 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); + 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_set_ref(guidance_h.sp.pos, sp_speed, sp_accel); + } + else if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) { struct FloatVect2 sp_accel_local; sp_accel_local.x = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.x); sp_accel_local.y = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.y); @@ -293,37 +302,44 @@ static void guidance_h_update_reference(void) /* either use the reference or simply copy the pos setpoint */ 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_RSHIFT(guidance_h.ref.pos, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_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 { - 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; + if (guidance_h.mode == GUIDANCE_H_MODE_NAV) { + 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_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; + 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; + } + } else { + // if not in NAV, copy setpoint to ref + VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos); + VECT2_COPY(guidance_h.ref.speed, guidance_h.sp.speed); + VECT2_COPY(guidance_h.ref.accel, guidance_h.sp.accel); } } @@ -447,7 +463,13 @@ struct StabilizationSetpoint guidance_h_guided_run(bool in_flight) guidance_h_update_reference(); - guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h); + if (guidance_h.sp.h_mask == GUIDANCE_H_SP_POS || guidance_h.sp.h_mask == GUIDANCE_H_SP_ALL) { + guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h); + } else { + // if not position control, run speed control (accel not supported in guided mode) + guidance_h_cmd = guidance_h_run_speed(in_flight, &guidance_h); + } + /* return final attitude setpoint */ return guidance_h_cmd; } @@ -511,3 +533,14 @@ void guidance_h_set_heading_rate(float rate) guidance_h.sp.heading_rate = rate; } +void guidance_h_set_all(float x, float y, float vx, float vy, float ax, float ay) +{ + guidance_h.sp.h_mask = GUIDANCE_H_SP_ALL; + guidance_h.sp.pos.x = POS_BFP_OF_REAL(x); + guidance_h.sp.pos.y = POS_BFP_OF_REAL(y); + guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx); + guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy); + guidance_h.sp.accel.x = ACCEL_BFP_OF_REAL(ax); + guidance_h.sp.accel.y = ACCEL_BFP_OF_REAL(ay); +} + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index a32a6fb8d6..d9694a1c3a 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -80,7 +80,8 @@ struct HorizontalGuidanceSetpoint { enum { GUIDANCE_H_SP_POS = 0, GUIDANCE_H_SP_SPEED = 1, - GUIDANCE_H_SP_ACCEL = 2 + GUIDANCE_H_SP_ACCEL = 2, + GUIDANCE_H_SP_ALL = 3 } h_mask; enum { GUIDANCE_H_SP_YAW = 0, @@ -152,25 +153,35 @@ 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); +extern void guidance_h_set_vel(float vx, float vy); /** Set body relative horizontal acceleration setpoint. - * @param vx forward acceleration (body frame) in meters/sec². - * @param vy right acceleration (body frame) in meters/sec². + * @param ax forward acceleration (body frame) in meters/sec². + * @param ay 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². + * @param ax North acceleration (local NED frame) in meters/sec². + * @param ay East acceleration (local NED frame) in meters/sec². */ -extern void guidance_h_set_vel(float vx, float vy); +extern void guidance_h_set_acc(float ax, float ay); /** Set heading rate setpoint. * @param rate Heading rate in radians. */ extern void guidance_h_set_heading_rate(float rate); +/** Set position speed and acceleration (NED frame). + * @param x North position (local NED frame) in meters. + * @param y East position (local NED frame) in meters. + * @param vx North velocity (local NED frame) in meters/sec. + * @param vy East velocity (local NED frame) in meters/sec. + * @param ax North acceleration (local NED frame) in meters/sec². + * @param ay East acceleration (local NED frame) in meters/sec². + */ +extern void guidance_h_set_all(float x, float y, float vx, float vy, float ax, float ay); + /* Make sure that ref can only be temporarily disabled for testing, * but not enabled if GUIDANCE_H_USE_REF was defined to FALSE. */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 0f63b6622b..e9dfce3f47 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -64,6 +64,7 @@ static bool desired_zd_updated; #define GUIDANCE_V_GUIDED_MODE_ZHOLD 0 #define GUIDANCE_V_GUIDED_MODE_CLIMB 1 #define GUIDANCE_V_GUIDED_MODE_THROTTLE 2 +#define GUIDANCE_V_GUIDED_MODE_ALL 3 static int guidance_v_guided_mode; @@ -145,6 +146,7 @@ void guidance_v_mode_changed(uint8_t new_mode) case GUIDANCE_V_MODE_RC_CLIMB: case GUIDANCE_V_MODE_CLIMB: guidance_v.zd_sp = 0; + guidance_v.zdd_sp = 0; /* Falls through. */ case GUIDANCE_V_MODE_NAV: guidance_v_run_enter(); @@ -275,6 +277,7 @@ void guidance_v_z_enter(void) /* reset speed setting */ guidance_v.zd_sp = 0; + guidance_v.zdd_sp = 0; } void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel) @@ -304,18 +307,21 @@ struct ThrustSetpoint guidance_v_from_nav(bool in_flight) if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) { guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude); guidance_v.zd_sp = 0; + guidance_v.zdd_sp = 0; gv_update_ref_from_z_sp(guidance_v.z_sp); guidance_v_update_ref(); sp = guidance_v_run_pos(in_flight, &guidance_v); } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) { guidance_v.z_sp = stateGetPositionNed_i()->z; guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb); + guidance_v.zdd_sp = 0; gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z); guidance_v_update_ref(); sp = guidance_v_run_speed(in_flight, &guidance_v); } else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) { guidance_v.z_sp = stateGetPositionNed_i()->z; guidance_v.zd_sp = stateGetSpeedNed_i()->z; + guidance_v.zdd_sp = 0; GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0); guidance_v_run_enter(); sp = th_sp_from_thrust_i((int32_t)nav.throttle, THRUST_AXIS_Z); @@ -359,6 +365,11 @@ struct ThrustSetpoint guidance_v_guided_run(bool in_flight) guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only sp = th_sp_from_thrust_i(guidance_v.th_sp, THRUST_AXIS_Z); break; + case GUIDANCE_V_GUIDED_MODE_ALL: + // update full reference + guidance_v_set_ref(guidance_v.z_sp, guidance_v.zd_sp, guidance_v.zdd_sp); + sp = guidance_v_run_pos(in_flight, &guidance_v); + break; default: break; } @@ -371,8 +382,9 @@ void guidance_v_set_z(float z) guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD; /* set altitude setpoint */ guidance_v.z_sp = POS_BFP_OF_REAL(z); - /* reset speed setting */ + /* reset speed and accel setting */ guidance_v.zd_sp = 0; + guidance_v.zdd_sp = 0; } void guidance_v_set_vz(float vz) @@ -381,6 +393,8 @@ void guidance_v_set_vz(float vz) guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB; /* set speed setting */ guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz); + /* reset accel setpoint */ + guidance_v.zdd_sp = 0; } void guidance_v_set_th(float th) @@ -394,4 +408,11 @@ void guidance_v_set_th(float th) Bound(guidance_v.th_sp, 0, MAX_PPRZ); } +void guidance_v_set_all(float z, float vz, float az) +{ + guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD; + guidance_v.z_sp = POS_BFP_OF_REAL(z); + guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz); + guidance_v.zdd_sp = ACCEL_BFP_OF_REAL(az); +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 0ee3f173a4..61ffd15033 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -55,6 +55,12 @@ struct VerticalGuidance { */ int32_t zd_sp; + /** vertical acceleration setpoint in meter/s^2. + * fixed point representation: Q21.10 + * accuracy 0.0009766, range 2097152 + */ + int32_t zdd_sp; + /** altitude reference in meters. * fixed point representation: Q23.8 * accuracy 0.0039, range 8388km @@ -155,4 +161,11 @@ extern void guidance_v_set_vz(float vz); */ extern void guidance_v_set_th(float th); +/** Set z position, velocity and acceleration setpoint. + * @param z Setpoint (down is positive) in meters. + * @param vz Setpoint (down is positive) in meters/second. + * @param az Setpoint (down is positive) in meters/second². + */ +extern void guidance_v_set_all(float z, float vz, float az); + #endif /* GUIDANCE_V_H */ diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index c7f6b6325f..f0abfd1a01 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit c7f6b6325f2786222074e6c0ca13d294dda9a2f1 +Subproject commit f0abfd1a0115f8f596a18a27ddae90fc736f6aea diff --git a/sw/lib/python/guided_mode.py b/sw/lib/python/guided_mode.py index 8be08f7b0f..c48fab1d19 100755 --- a/sw/lib/python/guided_mode.py +++ b/sw/lib/python/guided_mode.py @@ -125,6 +125,24 @@ class GuidedMode(object): """ self.guided_cmd(ac_id, self.FLAG_XY_BODY | self.FLAG_XY_VEL | self.FLAG_Z_VEL | self.FLAG_YAW_VEL, forward, right, down, yaw_rate) + def set_full_ned(self, ac_id, x, y, z, vx=0.0, vy=0.0, vz=0.0, ax=0.0, ay=0.0, az=0.0, heading=0.0): + """ + set full reference for trajectory following in NED frame + """ + msg = PprzMessage("datalink", "GUIDED_FULL_NED") + msg['ac_id'] = ac_id + msg['x'] = x + msg['y'] = y + msg['z'] = z + msg['vx'] = vx + msg['vy'] = vy + msg['vz'] = vz + msg['ax'] = ax + msg['ay'] = ay + msg['az'] = az + msg['heading'] = heading + self._interface.send_raw_datalink(msg) + def main():