Implement classic GVF for rotorcrafts (#3327)

* [modules] implemented GVF for rotorcrafts

* Note about Crazyradio communication with Crazyflies

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"
   />
+  <aircraft
+   name="bebop2_gvf"
+   ac_id="204"
+   airframe="airframes/examples/bebop2_gvf.xml"
+   radio="radios/dummy.xml"
+   telemetry="telemetry/GVF/gvf_default_rotorcraft.xml"
+   flight_plan="flight_plans/demo_gvf_rotorcraft.xml"
+   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
+   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/gvf_module.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
+   gui_color="red"
+  />
   <aircraft
    name="bebop2_opticflow"
    ac_id="3"

conf/flight_plans/demo_gvf_rotorcraft.xml

@@ -0,0 +1,88 @@
+
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan alt="720.5" ground_alt="720" lat0="37.296981" lon0="-3.683133" max_dist_from_home="200" name="Rotorcraft GVF" security_height="0.1">
+
+  <waypoints>
+    <waypoint name="HOME" x="0.0" y="0.0"/>
+    <waypoint name="STDBY" x="0.1" y="0.8"/>
+    <waypoint name="S1_START" x="0.0" y="1.0"/>
+    <waypoint name="S1_END" x="0.0" y="5.0"/>
+    <waypoint name="S2_START" x="2.0" y="1.0"/>
+    <waypoint name="S2_END" x="2.0" y="5.0"/>
+    <waypoint name="TD" x="-1.0" y="1.5"/>
+  </waypoints>
+
+  <variables>
+    <variable init="0.1" var="fp_throttle"/>
+    <variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
+    <variable var="nominal_alt" init="0.5" type="float" min="0." max="10." step="0.1"/>
+  </variables>
+
+  <modules>
+    <module name="gvf_module"/>
+    <!-- module name="gvf_parametric"/--> <!-- Not implemented yet -->
+  </modules>
+
+  <blocks>
+    <block name="FPInit">
+      <call_once fun="NavKillThrottle()"/>
+      <while cond="!GpsFixValid()"/>
+      <while cond="TRUE"/>
+    </block>
+
+    <block name="Holding point">
+      <call_once fun="NavKillThrottle()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+
+    <block name="Start Engine" strip_button="Start Engine" strip_icon="resurrect.png" group="home">
+      <call_once fun="NavResurrect()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+
+    <block name="Take off" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
+      <call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
+    </block>
+
+    <block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
+      <stay wp="STDBY"/>
+    </block>
+
+    <block name="GVF Segment 1">
+      <call fun="gvf_segment_loop_wp1_wp2(WP_S1_START, WP_S1_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
+    </block>
+
+    <block name="GVF Segment 2">
+      <call fun="gvf_segment_loop_wp1_wp2(WP_S2_START, WP_S2_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
+    </block>
+
+    <block name="GVF 25m Circle">
+      <call fun="gvf_ellipse_wp(WP_TD, 25, 25, gvf_ellipse_par.alpha)"/>
+    </block>
+
+    <!-- Function found in ./sw/airborne/modules/guidance/gfv/gvf.h -->
+    <block name="GVF Ellipse">
+      <call fun="gvf_ellipse_wp(WP_TD, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
+    </block>
+
+    <block name="Land here" key="l" strip_button="Land Here" strip_icon="land-right.png">
+      <call_once fun="NavSetWaypointHere(WP_TD)"/>
+    </block>
+
+    <block name="Land">
+      <go wp="TD"/>
+    </block>
+
+    <block name="Land Target" strip_button="Land Target" group="target">
+      <exception cond="!nav_is_in_flight()" deroute="Kill Engine"/>
+      <stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
+    </block>
+
+    <block name="Kill Engine" key="k">
+      <call_once fun="NavKillThrottle()"/>
+      <while cond="1"/>
+    </block>
+  </blocks>
+
+</flight_plan>

conf/modules/gvf_module.xml

@@ -33,12 +33,14 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
       <dl_settings NAME="GVF">
         <dl_settings NAME="Control">
           <dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_control.s" shortname = "direction"/>
+          <dl_setting MAX="1" MIN="0" STEP="1" VAR="gvf_control.align" shortname = "(ROTORCRAFT) align with trajectory"/>
+          <dl_setting MAX="20" MIN="0.0" STEP="0.2" VAR="gvf_control.speed" shortname = "(ROTORCRAFT) speed"/>
         </dl_settings>
         <dl_settings NAME="Ellipse">
           <dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/>
           <dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.kn" shortname="ell_kn" param="GVF_ELLIPSE_KN"/>
-          <dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
-          <dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
+          <dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
+          <dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
           <dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="ell_alpha" param="GVF_ELLIPSE_ALPHA"/>
         </dl_settings>
           <dl_settings NAME="Line">

conf/modules/nav_rotorcraft.xml

@@ -9,7 +9,7 @@
   <settings>
     <dl_settings>
       <dl_settings NAME="NAV">
-        <dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/>
+        <dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="999" module="navigation" unit="m" handler="SetFlightAltitude"/>
         <dl_setting var="nav.heading" MIN="0" STEP="0.1" MAX="360" unit="rad" alt_unit="deg"/>
         <dl_setting var="nav.radius" MIN="-50" STEP="0.1" MAX="50" unit="m"/>
         <dl_setting var="nav.climb_vspeed" MIN="0" STEP="0.1" MAX="10.0" unit="m/s" param="NAV_CLIMB_VSPEED"/>

conf/telemetry/GVF/gvf_default_rotorcraft.xml

@@ -0,0 +1,216 @@
+<?xml version="1.0"?>
+<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
+<telemetry>
+
+
+  <process name="Main">
+
+    <mode name="default" key_press="d">
+      <message name="AUTOPILOT_VERSION"        period="11.1"/>
+      <message name="DL_VALUE"                 period="1.1"/>
+      <message name="ROTORCRAFT_STATUS"        period="1.2"/>
+      <message name="ROTORCRAFT_FP"            period="0.25"/>
+      <message name="ALIVE"                    period="2.1"/>
+      <message name="INS_REF"                  period="5.1"/>
+      <message name="ROTORCRAFT_NAV_STATUS"    period="1.6"/>
+      <message name="WP_MOVED"                 period="1.3"/>
+      <message name="ROTORCRAFT_CAM"           period="1."/>
+      <message name="GPS_INT"                  period=".25"/>
+      <message name="INS"                      period=".25"/>
+      <message name="GVF"                      period="3."/>
+      <message name="GVF_PARAMETRIC"           period="0.5"/>
+      <message name="I2C_ERRORS"               period="4.1"/>
+      <message name="UART_ERRORS"              period="3.1"/>
+      <message name="SUPERBITRF"               period="3"/>
+      <message name="ENERGY"                   period="2.5"/>
+      <message name="DATALINK_REPORT"          period="5.1"/>
+      <message name="STATE_FILTER_STATUS"      period="3.2"/>
+      <message name="AIR_DATA"                 period="1.3"/>
+      <message name="SURVEY"                   period="2.5"/>
+      <message name="OPTIC_FLOW_EST"           period="0.05"/>
+      <message name="VECTORNAV_INFO"           period="0.5"/>
+      <message name="OPTICAL_FLOW_HOVER"       period="0.05"/>
+      <message name="VISUALTARGET"             period="0.10"/>
+      <message name="VISION_POSITION_ESTIMATE" period="0.1"/>
+      <message name="DIVERGENCE"               period="0.05"/>
+      <message name="DRAGSPEED"                period="0.02"/>
+      <message name="LOGGER_STATUS"            period="5.1"/>
+      <message name="LIDAR"                    period="1.2"/>
+      <message name="INS_EKF2"                 period=".25"/>
+      <message name="WIND_INFO_RET"            period="1."/>
+      <message name="AHRS_REF_QUAT"            period="0.5"/>
+      <message name="STAB_ATTITUDE"            period=".25"/>
+      <message name="EFF_MAT_G"                period="2.0"/>
+    </mode>
+
+    <mode name="ppm">
+      <message name="DL_VALUE"                 period="0.5"/>
+      <message name="ALIVE"                    period="2.1"/>
+      <message name="ROTORCRAFT_CMD"           period=".05"/>
+      <message name="PPM"                      period="0.5"/>
+      <message name="RC"                       period="0.5"/>
+      <message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
+      <message name="ROTORCRAFT_STATUS"        period="1"/>
+      <message name="BEBOP_ACTUATORS"          period="0.2"/>
+    </mode>
+
+    <mode name="raw_sensors">
+      <message name="ROTORCRAFT_STATUS"           period="1.2"/>
+      <message name="DL_VALUE"                    period="0.5"/>
+      <message name="ALIVE"                       period="2.1"/>
+      <message name="IMU_ACCEL_RAW"               period=".05"/>
+      <message name="IMU_GYRO_RAW"                period=".05"/>
+      <message name="IMU_MAG_RAW"                 period=".05"/>
+      <message name="BARO_RAW"                    period=".1"/>
+      <message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
+      <message name="ARDRONE_NAVDATA"             period=".05"/>
+      <message name="AIRSPEED_RAW"                period="0.1"/>
+    </mode>
+
+    <mode name="scaled_sensors">
+      <message name="ROTORCRAFT_STATUS"      period="1.2"/>
+      <message name="DL_VALUE"               period="0.5"/>
+      <message name="ALIVE"                  period="2.1"/>
+      <message name="IMU_GYRO_SCALED"        period=".075"/>
+      <message name="IMU_ACCEL_SCALED"       period=".075"/>
+      <message name="IMU_MAG_SCALED"         period=".1"/>
+    </mode>
+
+    <mode name="ahrs">
+      <message name="ROTORCRAFT_STATUS"  period="1.2"/>
+      <message name="DL_VALUE"           period="0.5"/>
+      <message name="ALIVE"              period="2.1"/>
+      <message name="FILTER_ALIGNER"     period="2.2"/>
+      <message name="FILTER"             period=".5"/>
+      <message name="GEO_MAG"            period="5."/>
+      <message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
+      <message name="AHRS_QUAT_INT"      period=".25"/>
+      <message name="AHRS_EULER_INT"     period=".1"/>
+      <message name="AHRS_EULER"         period=".1"/>
+<!--      <message name="AHRS_RMAT_INT"   period=".5"/> -->
+    </mode>
+
+    <mode name="rate_loop">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="RATE_LOOP"         period=".02"/>
+    </mode>
+
+    <mode name="attitude_setpoint_viz" key_press="v">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="0.9"/>
+      <message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
+      <message name="AHRS_REF_QUAT" period="0.05"/>
+    </mode>
+
+    <mode name="attitude_loop" key_press="a">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="0.9"/>
+      <message name="STAB_ATTITUDE_INT"       period=".03"/>
+      <message name="STAB_ATTITUDE_REF_INT"   period=".03"/>
+      <message name="STAB_ATTITUDE_FLOAT"     period=".03"/>
+      <message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
+      <message name="STAB_ATTITUDE"           period=".25"/>
+      <message name="EFF_MAT_G"               period="2.0"/>
+    </mode>
+
+    <mode name="vert_loop" key_press="v">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="0.9"/>
+      <message name="VFF"               period=".05"/>
+      <message name="VFF_EXTENDED"      period=".05"/>
+      <message name="VERT_LOOP"         period=".05"/>
+      <message name="INS_Z"             period=".05"/>
+      <message name="INS"               period=".11"/>
+      <message name="INS_REF"           period="5.1"/>
+    </mode>
+
+    <mode name="vel_guidance" key_press="q">
+      <message name="ALIVE" period="0.9"/>
+      <message name="HYBRID_GUIDANCE" period="0.062"/>
+      <message name="GUIDANCE_INDI_HYBRID"     period="0.1"/>
+      <!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
+      <message name="ROTORCRAFT_FP" period="0.8"/>
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
+      <message name="INS_REF" period="5.1"/>
+      <message name="WP_MOVED" period="1.3"/>
+      <message name="GPS_INT" period="1.0"/>
+      <message name="INS" period="1.0"/>
+    </mode>
+
+    <mode name="h_loop" key_press="h">
+      <message name="ALIVE"                 period="0.9"/>
+      <message name="HOVER_LOOP"            period="0.062"/>
+      <message name="GUIDANCE_H_REF_INT"    period="0.062"/>
+      <message name="STAB_ATTITUDE_INT"     period="0.4"/>
+      <message name="STAB_ATTITUDE_FLOAT"   period="0.4"/>
+      <!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
+      <message name="ROTORCRAFT_FP"         period="0.8"/>
+      <message name="ROTORCRAFT_STATUS"     period="1.2"/>
+      <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
+      <message name="INS_REF"               period="5.1"/>
+      <!-- HFF messages are only sent if USE_HFF -->
+      <message name="HFF"                   period=".05"/>
+      <message name="HFF_GPS"               period=".03"/>
+      <message name="HFF_DBG"               period=".2"/>
+    </mode>
+
+    <mode name="aligner">
+      <message name="ALIVE"             period="0.9"/>
+      <message name="FILTER_ALIGNER"    period="0.02"/>
+    </mode>
+
+    <mode name="tune_hover">
+      <message name="DL_VALUE"          period="1.1"/>
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="GUIDANCE_H_INT"    period="0.05"/>
+      <message name="ROTORCRAFT_TUNE_HOVER"    period=".1"/>
+      <!-- <message name="GPS_INT"               period=".20"/> -->
+      <!--<message name="INS2"              period=".05"/>
+      <message name="INS3"              period=".20"/>-->
+      <message name="INS_REF"           period="5.1"/>
+    </mode>
+
+    <mode name="RTCM3" >
+      <message name="GPS_RXMRTCM"               period="1"/>
+      <message name="GPS_INT"                period=".25"/>
+      <message name="GPS_RTK"               period="1"/>
+    </mode>
+
+  </process>
+
+  <process name="FlightRecorder">
+    <mode name="default">
+      <message name="AUTOPILOT_VERSION"        period="11.1"/>
+      <message name="DL_VALUE"                 period="1.1"/>
+      <message name="ROTORCRAFT_STATUS"        period="1.2"/>
+      <message name="ROTORCRAFT_FP"            period="0.25"/>
+      <message name="INS_REF"                  period="5.1"/>
+      <message name="ROTORCRAFT_NAV_STATUS"    period="1.6"/>
+      <message name="WP_MOVED"                 period="1.3"/>
+      <message name="GPS_INT"                  period=".1"/>
+      <message name="INS"                      period=".1"/>
+      <message name="I2C_ERRORS"               period="4.1"/>
+      <message name="UART_ERRORS"              period="3.1"/>
+      <message name="ENERGY"                   period="2.5"/>
+      <message name="DATALINK_REPORT"          period="5.1"/>
+      <message name="STATE_FILTER_STATUS"      period="3.2"/>
+      <message name="AIR_DATA"                 period="1.3"/>
+      <message name="SURVEY"                   period="2.5"/>
+      <message name="IMU_GYRO_SCALED"          period=".075"/>
+      <message name="IMU_ACCEL_SCALED"         period=".075"/>
+      <message name="IMU_MAG_SCALED"           period=".2"/>
+      <message name="LIDAR"                    period="0.05"/>
+      <message name="ESC"                      period="0.05"/>
+      <message name="STAB_ATTITUDE"            period=".1"/>
+    </mode>
+  </process>
+
+</telemetry>
+

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;

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

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