From 63fd015f4d738aa5bcc0871aa9f0fcad3cdb3a49 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 6 Sep 2023 21:52:57 +0200 Subject: [PATCH] [modules] Fix follow me (#3055) * [misc] Ublox2Ivy update * [modules] Follow me moving waypoints and approach moving target handling unknown heading --- conf/modules/follow_me.xml | 11 +- conf/userconf/tudelft/conf.xml | 14 +-- .../modules/ctrl/approach_moving_target.c | 4 + sw/airborne/modules/ctrl/follow_me.c | 105 ++++++++++++++---- sw/airborne/modules/ctrl/follow_me.h | 5 +- sw/ground_segment/misc/ublox2ivy.c | 26 +++-- 6 files changed, 122 insertions(+), 43 deletions(-) diff --git a/conf/modules/follow_me.xml b/conf/modules/follow_me.xml index 3f84dd6e67..ebe812cb4b 100644 --- a/conf/modules/follow_me.xml +++ b/conf/modules/follow_me.xml @@ -4,17 +4,22 @@ 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.
+ + + + +
- + @@ -32,9 +37,9 @@ + - diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index ca92f17f60..e530466c40 100644 --- 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" /> 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); diff --git a/sw/airborne/modules/ctrl/follow_me.c b/sw/airborne/modules/ctrl/follow_me.c index 63a8e5af47..4527a47a5b 100644 --- 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(); } - diff --git a/sw/airborne/modules/ctrl/follow_me.h b/sw/airborne/modules/ctrl/follow_me.h index 59ea2c8d38..0a08620b93 100644 --- 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 - diff --git a/sw/ground_segment/misc/ublox2ivy.c b/sw/ground_segment/misc/ublox2ivy.c index 3bfe1ef708..32bfde5c77 100644 --- 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; -} \ No newline at end of file +}