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():