diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index 1c13c4c440..7acfb71b91 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -109,6 +109,17 @@
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/gvf_module.xml b/conf/modules/gvf_module.xml
index 98e623f06e..e9dbbd059a 100644
--- a/conf/modules/gvf_module.xml
+++ b/conf/modules/gvf_module.xml
@@ -33,12 +33,14 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
+
+
-
-
+
+
diff --git a/conf/modules/nav_rotorcraft.xml b/conf/modules/nav_rotorcraft.xml
index e7547780a0..534aa56f18 100644
--- a/conf/modules/nav_rotorcraft.xml
+++ b/conf/modules/nav_rotorcraft.xml
@@ -9,7 +9,7 @@
-
+
diff --git a/conf/telemetry/GVF/gvf_default_rotorcraft.xml b/conf/telemetry/GVF/gvf_default_rotorcraft.xml
new file mode 100644
index 0000000000..d851087e0c
--- /dev/null
+++ b/conf/telemetry/GVF/gvf_default_rotorcraft.xml
@@ -0,0 +1,216 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c
index 5f92eb5c22..284b7b11ed 100644
--- a/sw/airborne/modules/guidance/gvf/gvf.c
+++ b/sw/airborne/modules/guidance/gvf/gvf.c
@@ -118,6 +118,8 @@ void gvf_init(void)
gvf_control.ke = 1;
gvf_control.kn = 1;
gvf_control.s = 1;
+ gvf_control.speed = 1.0; // Rotorcraft only (for now)
+ gvf_control.align = false; // Rotorcraft only
gvf_trajectory.type = NONE;
#if PERIODIC_TELEMETRY
@@ -177,18 +179,18 @@ void gvf_control_2D(float ke, float kn, float e,
float md_dot_x = md_y * md_dot_const;
float md_dot_y = -md_x * md_dot_const;
+ #if defined(ROTORCRAFT_FIRMWARE)
- #ifdef ROTORCRAFT_FIRMWARE
+ // Use accel based control. Not recommended as of current implementation
+ #if defined(GVF_ROTORCRAFT_USE_ACCEL)
// Set nav for command
-
// Use parameter kn as the speed command
nav.speed.x = md_x * kn;
nav.speed.y = md_y * kn;
-
// Acceleration induced by the field with speed set to kn (!WIP!)
-#warning "Using GVF for rotorcraft is still experimental, proceed with caution"
+ #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
float n_norm = sqrtf(nx*nx+ny*ny);
float hess_px_dot = px_dot * H11 + py_dot * H12;
float hess_py_dot = px_dot * H21 + py_dot * H22;
@@ -205,12 +207,28 @@ void gvf_control_2D(float ke, float kn, float e,
float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
- // TODO don't change nav struct directly
+ // TODO: don't change nav struct directly
nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
nav.heading = atan2f(md_x,md_y);
- #else
+ #else // SPEED_BASED_GVF
+
+ nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
+
+ // Speed-based control, acceleration based control not implemented yet
+ nav.speed.x = gvf_control.speed * md_x;
+ nav.speed.y = gvf_control.speed * md_y;
+
+ // Optionally align heading with trajectory
+ if (gvf_control.align)
+ {
+ nav.heading = atan2f(md_x, md_y);
+ }
+
+ #endif
+
+ #else // FIXEDWING / ROVER FIRMWARE
float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
@@ -230,6 +248,21 @@ void gvf_control_2D(float ke, float kn, float e,
#endif
}
+// BEGIN ROTORCRAFT
+
+void gvf_set_speed(float speed)
+{
+ if (speed < 0.0) speed = 0.0;
+ gvf_control.speed = speed;
+}
+
+void gvf_set_align(bool align)
+{
+ gvf_control.align = align;
+}
+
+// END ROTORCRAFT
+
void gvf_set_direction(int8_t s)
{
gvf_control.s = s;
diff --git a/sw/airborne/modules/guidance/gvf/gvf.h b/sw/airborne/modules/guidance/gvf/gvf.h
index 8c2eb8450d..eaddc029f8 100644
--- a/sw/airborne/modules/guidance/gvf/gvf.h
+++ b/sw/airborne/modules/guidance/gvf/gvf.h
@@ -42,14 +42,19 @@
* @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field
* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
+* @param omega Angular velocity of the vehicle
+* @param speed Speed of the vehicle. It is only used in rotorcrafts for now.
* @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1.
+* @param align Align the vehicle with the direction of the vector field. Can only used in rotorcrafts.
*/
typedef struct {
float ke;
float kn;
float error;
float omega;
+ float speed;
int8_t s;
+ bool align;
} gvf_con;
extern gvf_con gvf_control;
@@ -113,9 +118,11 @@ struct gvf_Hess {
extern void gvf_init(void);
void gvf_control_2D(float ke, float kn, float e,
struct gvf_grad *, struct gvf_Hess *);
+extern void gvf_set_speed(float speed); // Rotorcraft only (for now)
+extern void gvf_set_align(bool align); // Rotorcraft only
extern void gvf_set_direction(int8_t s);
-// Straigh line
+// Straight line
extern bool gvf_line_XY_heading(float x, float y, float heading);
extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
@@ -125,7 +132,6 @@ extern bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
extern bool gvf_line_wp_heading(uint8_t wp, float heading);
-
// Ellipse
extern bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
extern bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha);
@@ -137,5 +143,4 @@ extern bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off,
extern bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off,
float A);
-
-#endif // GVF_H
+#endif // GVF_H
\ No newline at end of file
diff --git a/sw/ground_segment/python/bitcraze/crazyradio2ivy.py b/sw/ground_segment/python/bitcraze/crazyradio2ivy.py
index 09735b0fd5..6445bc798b 100755
--- a/sw/ground_segment/python/bitcraze/crazyradio2ivy.py
+++ b/sw/ground_segment/python/bitcraze/crazyradio2ivy.py
@@ -5,6 +5,11 @@ with support of PPRZLINK messages
Requires 'pip3 install cflib'
+IMPORTANT:
+To make this script work, you need to have the Crazyflie firmware 2019.09.
+Some other firmwares may work, but the 2019.09 is the one that has been tested.
+2024 firmwares onwards DO NOT work for sure.
+
As the ESB protocol works using PTX and PRX (Primary Transmitter/Reciever)
modes. Thus, data is only recieved as a response to a sent packet.
So, we need to constantly poll the receivers for bidirectional communication.
@@ -34,12 +39,10 @@ import pprzlink.messages_xml_map as messages_xml_map
CRTP_PORT_PPRZLINK = 9
-
# Only output errors from the logging framework
#logging.basicConfig(level=logging.DEBUG)
logging.basicConfig(level=logging.ERROR)
-
class RadioBridge:
def __init__(self, link_uri, msg_class='telemetry', verbose=False):
""" Initialize and run with the specified link_uri """
@@ -173,5 +176,4 @@ if __name__ == '__main__':
bridge.shutdown()
time.sleep(1)
- sys.exit()
-
+ sys.exit()
\ No newline at end of file