[modules] Fix follow me (#3055)

* [misc] Ublox2Ivy update * [modules] Follow me moving waypoints and approach moving target handling unknown heading
2026-03-23 15:34:18 +08:00 · 2023-09-06 21:52:57 +02:00
parent a267e39e82
commit 63fd015f4d
6 changed files with 122 additions and 43 deletions
--- a/conf/modules/follow_me.xml
+++ b/conf/modules/follow_me.xml
@@ -4,17 +4,22 @@
  <doc>
    <description>
      Follow a ground based object which is transmitting the TARGET_POS datalink message or using RTK GPS.
-      This can be done using gpsd2ivy.
+      This can be done using gpsd2ivy or ublox2ivy.
    </description>
    <section name="FOLLOW_ME" prefix="FOLLOW_ME_">
      <define name="TIMEOUT" value="10." description="timeout of the ground GPS"/>
      <define name="DISTANCE" value="5." description="distance to hover from the ground GPS"/>
+      <define name="HEIGHT" value="60" description="height to hover above the ground GPS"/>
+      <define name="GPS_TIMEOUT" value="5000" description="RTK relative position timeout in ms"/>
+      <define name="GROUND_TIMEOUT" value="5000" description="ground position timeout in ms"/>
+      <define name="FILT" value="0.9" description="filtering of the heading cos/sin (higher is harder)"/>
+      <define name="MOVING_WPS" value="" description="list of waypoints comma seperated which should move"/>
    </section>
  </doc>
  <settings>
    <dl_settings>
      <dl_settings NAME="FollowMe">
-        <dl_setting var="follow_me_distance" min="0." max="50." step="0.1" shortname="distance" unit="m"/>
+        <dl_setting var="follow_me_distance" min="0." max="400" step="1.0" shortname="distance" unit="m"/>
        <dl_setting var="follow_me_height" min="0." max="100." step="0.1" shortname="height" unit="m"/>
        <dl_setting var="follow_me_heading" min="0." max="360." step="1.0" shortname="heading"/>
        <dl_setting var="follow_me_min_speed" min="0." max="100." step="0.1" shortname="min_speed"/>
@@ -32,9 +37,9 @@
    <file name="follow_me.h"/>
  </header>
  <init fun="follow_me_init()"/>
+  <periodic fun="follow_me_periodic()" freq="1"/>
  <datalink message="TARGET_POS" fun="follow_me_parse_target_pos(buf)"/>
  <makefile>
    <file name="follow_me.c"/>
  </makefile>
 </module>
-
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -280,9 +280,9 @@
   airframe="airframes/tudelft/nederdrone4.xml"
   radio="radios/crossfire_sbus.xml"
   telemetry="telemetry/highspeed_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/nederdrone_troia.xml"
+   flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
   settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/approach_moving_target.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml modules/target_pos.xml"
+   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
   gui_color="blue"
  />
  <aircraft
@@ -291,9 +291,9 @@
   airframe="airframes/tudelft/nederdrone6.xml"
   radio="radios/crossfire_sbus.xml"
   telemetry="telemetry/highspeed_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
+   flight_plan="flight_plans/tudelft/nederdrone_harde.xml"
   settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
   gui_color="blue"
  />
  <aircraft
@@ -302,9 +302,9 @@
   airframe="airframes/tudelft/nederdrone7.xml"
   radio="radios/crossfire_sbus.xml"
   telemetry="telemetry/highspeed_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/nederdrone_terschelling.xml"
+   flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
   settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
   gui_color="blue"
  />
  <aircraft
@@ -315,7 +315,7 @@
   telemetry="telemetry/highspeed_rotorcraft.xml"
   flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
   settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
   gui_color="#ffff00000000"
  />
  <aircraft
--- a/sw/airborne/modules/ctrl/approach_moving_target.c
+++ b/sw/airborne/modules/ctrl/approach_moving_target.c
@@ -122,6 +122,10 @@ void follow_diagonal_approach(void) {
    // TODO: What to do? Same can be checked for the velocity
    return;
  }
+  if(target_heading > 360.f) {
+    // TODO: We got an invalid heading and need to do something with it
+    return;
+  }
  target_get_vel(&target_vel);
  VECT3_SMUL(target_vel, target_vel, amt.speed_gain);

--- a/sw/airborne/modules/ctrl/follow_me.c
+++ b/sw/airborne/modules/ctrl/follow_me.c
@@ -27,20 +27,16 @@

 #include "modules/datalink/telemetry.h"
 #include "generated/modules.h"
+#include "generated/flight_plan.h"

-// Distance to the target to hover from is by default 15 meters
+// Distance to the target to hover from is by default 45 meters
 #ifndef FOLLOW_ME_DISTANCE
 #define FOLLOW_ME_DISTANCE 45
 #endif

-// Height difference between te target be default 18 meters
+// Height difference between te target be default 60 meters
 #ifndef FOLLOW_ME_HEIGHT
-#define FOLLOW_ME_HEIGHT 75
-#endif
-
-// Minimum speed in m/s which the ground needs to have in order to update the heading
-#ifndef FOLLOW_ME_MIN_SPEED
-#define FOLLOW_ME_MIN_SPEED 1.0f
+#define FOLLOW_ME_HEIGHT 60
 #endif

 // The relative position GPS timeout in ms
@@ -53,15 +49,19 @@
 #define FOLLOW_ME_GROUND_TIMEOUT 5000
 #endif

-// The default course sin/cos filter value (higher is harder filtering)
+// The default heading sin/cos filter value (higher is harder filtering)
 #ifndef FOLLOW_ME_FILT
 #define FOLLOW_ME_FILT 0.9
 #endif

+// By default no moving waypoints
+#ifndef FOLLOW_ME_MOVING_WPS
+#define FOLLOW_ME_MOVING_WPS
+#endif
+
 float follow_me_distance = FOLLOW_ME_DISTANCE;
 float follow_me_height = FOLLOW_ME_HEIGHT;
-float follow_me_heading = 0.;
-float follow_me_min_speed = FOLLOW_ME_MIN_SPEED;
+float follow_me_heading = 180.;
 float follow_me_filt = FOLLOW_ME_FILT;
 float follow_me_diag_speed = 1.0;
 float follow_me_gps_delay = 200;
@@ -76,11 +76,72 @@ static struct LlaCoor_i ground_lla;
 static float ground_speed;
 static float ground_climb;
 static float ground_course;
+static float ground_heading;
+
+static uint8_t moving_wps[] = {FOLLOW_ME_MOVING_WPS};
+static uint8_t moving_wps_cnt = 0;
+static struct EnuCoor_f last_targetpos;
+static float last_targetpos_heading;
+static bool last_targetpos_valid = false;

 void follow_me_init(void)
 {
  ground_set = false;
  ground_time_msec = 0;
+  last_targetpos_valid = false;
+  moving_wps_cnt = sizeof(moving_wps) / sizeof(uint8_t);
+}
+
+void follow_me_periodic(void)
+{
+  if(!ground_set) {
+    return;
+  }
+
+  // Calculate the difference to move the waypoints
+  struct EnuCoor_i target_pos_cm;
+  struct EnuCoor_f cur_targetpos, diff_targetpos;
+  float cur_targetpos_heading, diff_targetpos_heading;
+
+  enu_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &ground_lla);
+  VECT3_FLOAT_OF_CM(cur_targetpos, target_pos_cm);
+  VECT3_DIFF(diff_targetpos, cur_targetpos, last_targetpos);
+
+  cur_targetpos_heading = ground_heading;
+  diff_targetpos_heading = cur_targetpos_heading - last_targetpos_heading;
+
+  // Only move if we had a previous location
+  VECT3_COPY(last_targetpos, cur_targetpos);
+  last_targetpos_heading = cur_targetpos_heading;
+  if(!last_targetpos_valid) {
+    last_targetpos_valid = true;
+    return;
+  }
+
+  // Go through all waypoints
+  for(uint8_t i = 0; i < moving_wps_cnt; i++) {
+    uint8_t wp_id = moving_wps[i];
+    struct EnuCoor_f *wp_enu = waypoint_get_enu_f(wp_id);
+    struct EnuCoor_f wp_new_enu;
+    wp_new_enu.x = wp_enu->x + diff_targetpos.x;
+    wp_new_enu.y = wp_enu->y + diff_targetpos.y;
+    wp_new_enu.z = wp_enu->z;
+
+    // Rotate the waypoint
+    float cos_heading = cosf(diff_targetpos_heading/180.*M_PI);
+    float sin_heading = sinf(diff_targetpos_heading/180.*M_PI);
+    wp_new_enu.x = ((wp_new_enu.x - cur_targetpos.x) * cos_heading) + ((wp_new_enu.y - cur_targetpos.y) * sin_heading) +  + cur_targetpos.x;
+    wp_new_enu.y = (-(wp_new_enu.y - cur_targetpos.y) * sin_heading) + ((wp_new_enu.y - cur_targetpos.y) * cos_heading) + cur_targetpos.y;
+
+    // Update the waypoint
+    waypoint_set_enu(wp_id, &wp_new_enu);
+
+    // Send to the GCS that the waypoint has been moved
+    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
+                               &waypoints[wp_id].enu_i.x,
+                               &waypoints[wp_id].enu_i.y,
+                               &waypoints[wp_id].enu_i.z);
+  }
 }

 void follow_me_parse_target_pos(uint8_t *buf)
@@ -96,12 +157,13 @@ void follow_me_parse_target_pos(uint8_t *buf)
  ground_speed = DL_TARGET_POS_speed(buf);
  ground_climb = DL_TARGET_POS_climb(buf);
  ground_course = DL_TARGET_POS_course(buf);
-
-  // Update the heading based on the course
-  if(ground_speed > follow_me_min_speed) {
-    follow_me_heading = ground_course + 180.f;
-    if(follow_me_heading > 360.f) follow_me_heading -= 360.f;
+  ground_heading = DL_TARGET_POS_heading(buf);
+  if(ground_heading > 360.f) {
+    // Ground heading is invalid
+    ground_set = false;
+    return;
  }
+
  ground_set = true;
 }

@@ -111,8 +173,8 @@ void follow_me_set_wp(uint8_t wp_id, float speed)
  struct NedCoor_f target_pos;
  float diff_time_ms = 0;

-  // Check if we got a valid relative position which didn't timeout
-  if(bit_is_set(gps.valid_fields, GPS_VALID_RELPOS_BIT) && gps.relpos_tow+FOLLOW_ME_GPS_TIMEOUT > gps_tow_from_sys_ticks(sys_time.nb_tick)) {
+  // Check if we got a valid relative position which didn't timeout (FIXME)
+  /*if(bit_is_set(gps.valid_fields, GPS_VALID_RELPOS_BIT) && gps.relpos_tow+FOLLOW_ME_GPS_TIMEOUT > gps_tow_from_sys_ticks(sys_time.nb_tick)) {
    static struct NedCoor_f cur_pos;
    static uint32_t last_relpos_tow = 0;

@@ -132,7 +194,7 @@ void follow_me_set_wp(uint8_t wp_id, float speed)
    if(diff_time_ms < 0) diff_time_ms += (1000*60*60*24*7); //msec of a week
  }
  // Check if we got a position from the ground which didn't timeout and local NED is initialized
-  else if(ground_set && state.ned_initialized_i && ground_time_msec+FOLLOW_ME_GROUND_TIMEOUT > get_sys_time_msec()) {
+  else*/ if(ground_set && state.ned_initialized_i && ground_time_msec+FOLLOW_ME_GROUND_TIMEOUT > get_sys_time_msec()) {
    struct NedCoor_i target_pos_cm;
    ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &ground_lla);
    target_pos.x = target_pos_cm.x / 100.;
@@ -184,8 +246,8 @@ void follow_me_set_wp(uint8_t wp_id, float speed)
    }

    // Filter the cosine and sine of the follow me heading to avoid wrapping
-    fmh_cos_filt = fmh_cos_filt * follow_me_filt + cosf(follow_me_heading/180.*M_PI) * (1 - follow_me_filt);
-    fmh_sin_filt = fmh_sin_filt * follow_me_filt + sinf(follow_me_heading/180.*M_PI) * (1 - follow_me_filt);
+    fmh_cos_filt = fmh_cos_filt * follow_me_filt + cosf((ground_heading+follow_me_heading)/180.*M_PI) * (1 - follow_me_filt);
+    fmh_sin_filt = fmh_sin_filt * follow_me_filt + sinf((ground_heading+follow_me_heading)/180.*M_PI) * (1 - follow_me_filt);

    // Add the target distance in the direction of the follow me heading
    target_pos.x += dist * fmh_cos_filt;
@@ -207,4 +269,3 @@ void follow_me_set_wp(uint8_t wp_id, float speed)
  // Allways update the time to avoid big jumps in distance and height
  last_time_ms = get_sys_time_msec();
 }
-
--- a/sw/airborne/modules/ctrl/follow_me.h
+++ b/sw/airborne/modules/ctrl/follow_me.h
@@ -76,6 +76,10 @@ extern float follow_me_min_height;
 */
 extern void follow_me_init(void);

+/** periodic function
+ */
+extern void follow_me_periodic(void);
+
 /** on receiving a TARGET_POS message
 */
 extern void follow_me_parse_target_pos(uint8_t *buf);
@@ -94,4 +98,3 @@ extern void follow_me_parse_target_pos(uint8_t *buf);
 extern void follow_me_set_wp(uint8_t wp_id, float speed);

 #endif
-
--- a/sw/ground_segment/misc/ublox2ivy.c
+++ b/sw/ground_segment/misc/ublox2ivy.c
@@ -43,6 +43,7 @@

 #define UDP_BUFFER_SIZE   1024
 #define NAV_RELPOSNED_VERSION   0x01
+#define INVALID_HEADING  361

 /* Endpoints */
 struct endpoint_udp_t {
@@ -78,11 +79,12 @@ struct endpoint_t {
 };

 static bool verbose = false;
+static bool rtcm_forward = true;
 static struct endpoint_t gps_ep;
 static struct gps_ubx_t gps_ubx;
 static struct gps_rtcm_t gps_rtcm;
 static uint8_t ac_id = 0;
-static float ground_heading = NAN;
+static float ground_heading = INVALID_HEADING;

 void packet_handler(void *ep, uint8_t *data, uint16_t len);

@@ -631,17 +633,18 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
          if(version > NAV_RELPOSNED_VERSION)
            break;

+          uint16_t refStationId = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf);
          uint8_t flags       = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
          uint8_t relPosValid = RTCMgetbitu(&flags, 5, 1);
          float relpos_heading = UBX_NAV_RELPOSNED_relPosHeading(gps_ubx.msg_buf) * 1e-5f;
          float relpos_dist = UBX_NAV_RELPOSNED_relPosLength(gps_ubx.msg_buf) * 1e-2f;

-          if(verbose) printf("Got relpos %d %f %f\r\n", flags, relpos_heading, relpos_dist);
+          if(verbose) printf("Got relpos %d %d %f %f\r\n", refStationId, flags, relpos_heading, relpos_dist);

          if(relPosValid) {
            ground_heading = relpos_heading;
          } else {
-            ground_heading = NAN;
+            ground_heading = INVALID_HEADING;
          }
          
          break;
@@ -661,7 +664,8 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
      if(verbose) printf("Got a succesfull RTCM message [%d]\r\n", RTCMgetbitu(gps_rtcm.msg_buf, 24 + 0, 12));

      /* Forward the message to inject into the drone */
-      send_gps_inject(gps_rtcm.msg_buf, gps_rtcm.len + 6);
+      if(rtcm_forward)
+        send_gps_inject(gps_rtcm.msg_buf, gps_rtcm.len + 6);

      gps_rtcm.msg_available = false;
    }
@@ -686,6 +690,7 @@ int main(int argc, char** argv) {
  static struct option long_options[] = {
    {"ac_id", required_argument, NULL, 'i'},
    {"endpoint", required_argument, NULL, 'e'},
+    {"rtcm_disable", no_argument, NULL, 'r'},
    {"help", no_argument, NULL, 'h'},
    {"verbose", no_argument, NULL, 'v'},
    {0, 0, 0, 0}
@@ -695,12 +700,13 @@ int main(int argc, char** argv) {
    " Options :\n"
    "   -i --ac_id [aircraft_id]               Aircraft id\n"
    "   -e --endpoint [endpoint_str]           Endpoint address of the GPS\n"
+    "   -r --rtcm_disable                      Disables RTCM forwarding"
    "   -h --help                              Display this help\n"
    "   -v --verbose                           Print verbose information\n";

  int c;
  int option_index = 0;
-  while((c = getopt_long(argc, argv, "i:e:hv", long_options, &option_index)) != -1) {
+  while((c = getopt_long(argc, argv, "i:e:rhv", long_options, &option_index)) != -1) {
    switch (c) {
      case 'i':
        ac_id = atoi(optarg);
@@ -733,6 +739,10 @@ int main(int argc, char** argv) {
        }
        break;

+      case 'r':
+        rtcm_forward = false;
+        break;
+
      case 'v':
        verbose = true;
        break;
@@ -750,9 +760,5 @@ int main(int argc, char** argv) {

  g_main_loop_run(ml);

-  /*while(true) {
-    usleep(50000);
-  }*/
-
  return 0;
-}
+}