From 745940605e7f5912292efc3b617e89abb9df5e40 Mon Sep 17 00:00:00 2001 From: alfredoFBW Date: Wed, 17 Jul 2024 16:11:10 +0200 Subject: [PATCH 1/2] Fixed error of a big integration step when vehicle stopped in gvf_parametric_bare --- .../gvf_parametric_bare/gvf_parametric_bare.c | 28 +++++++++++++++++-- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c index cc3fdab2c42..381915048ed 100644 --- a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c +++ b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c @@ -118,9 +118,31 @@ void gvf_parametric_bare_control_2D(float kx, float ky, float f1, float f2, floa gvf_parametric_bare_control.delta_T = now - gvf_parametric_bare_t0; gvf_parametric_bare_t0 = now; - if (gvf_parametric_bare_control.delta_T > 300) { // We need at least two iterations for Delta_T - gvf_parametric_bare_control.w = 0; // Reset w since we assume the algorithm starts - return; + /* If the vehicle does not need to stop at the wp */ + if(!gvf_c_stopwp.stop_at_wp) + { + // We need at least two iterations for Delta_T + if ((gvf_parametric_bare_control.delta_T > 300)) + { + /* Reset w since we assume the algorithm starts, because there cannot be any + * physical stop of 300 ms */ + gvf_parametric_bare_control.w = 0; + return; + } + } + else + { + /* TODO: Replace magic number for conversion from ms to s and the number + * of seconds of error */ + /* If the vehicle has to stop at any waypoint wp, it shall wait outside + * gvf_parametric_bare, gvf_c_stopwp.wait_time seconds, so when coming back + * into this function, delta_T could be really big. If that's the case then + * fix delta_T to a value, so the integration step is properly carried. + */ + if((gvf_parametric_bare_control.delta_T >= (gvf_c_stopwp.wait_time - 1)* 1000)) + { + gvf_parametric_bare_control.delta_T = 1.0 / PERIODIC_FREQUENCY; + } } // Carrot position From 20ac959dba4fa0473b1599d49f0e5385da8ae881 Mon Sep 17 00:00:00 2001 From: alfredoFBW Date: Thu, 18 Jul 2024 17:05:57 +0200 Subject: [PATCH 2/2] Added global variable for gvf cpu time computation according to review comments --- .../rover/guidance/rover_guidance_steering.c | 3 +- sw/airborne/modules/guidance/gvf_common.c | 1 + sw/airborne/modules/guidance/gvf_common.h | 3 ++ .../gvf_parametric_bare/gvf_parametric_bare.c | 38 +++++-------------- .../gvf_parametric_bare/gvf_parametric_bare.h | 2 + 5 files changed, 17 insertions(+), 30 deletions(-) diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c index 3d0b47623bb..547626af8cf 100644 --- a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c +++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c @@ -392,7 +392,8 @@ bool rover_guidance_bearing_static_ctrl(void) guidance_control.cmd.speed = 0.0; guidance_control.cmd.delta = 0.0; //guidance_control.throttle = BoundThrottle(0.0); - + /* Obtain current cpu time when in static control */ + gvf_c_t0 = get_sys_time_msec(); return true; } diff --git a/sw/airborne/modules/guidance/gvf_common.c b/sw/airborne/modules/guidance/gvf_common.c index a6a5df80578..258219a151b 100644 --- a/sw/airborne/modules/guidance/gvf_common.c +++ b/sw/airborne/modules/guidance/gvf_common.c @@ -23,3 +23,4 @@ gvf_common_omega gvf_c_omega; gvf_common_params gvf_c_info; gvf_common_stop_at_wp gvf_c_stopwp; +uint32_t gvf_c_t0; diff --git a/sw/airborne/modules/guidance/gvf_common.h b/sw/airborne/modules/guidance/gvf_common.h index cac98d3799a..00141e830f3 100644 --- a/sw/airborne/modules/guidance/gvf_common.h +++ b/sw/airborne/modules/guidance/gvf_common.h @@ -39,6 +39,7 @@ extern gvf_common_omega gvf_c_omega; * @brief Different parameters obtained from gvfs. dot means d/dt * @param kappa is the curve's curvature * @param ori_err is the orientation error +* #param ori_err_dot is the derivative of the orientation error */ typedef struct{ float kappa; @@ -59,6 +60,8 @@ typedef struct{ extern gvf_common_params gvf_c_info; extern gvf_common_stop_at_wp gvf_c_stopwp; +/* @ brief Variable to keep the current cpu time in gvf calls */ +extern uint32_t gvf_c_t0; #endif // GVF_COMMON_H diff --git a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c index 381915048ed..32d73279531 100644 --- a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c +++ b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c @@ -34,7 +34,6 @@ // Control -uint32_t gvf_parametric_bare_t0 = 0; // We need it for calculting the time lapse delta_T uint32_t gvf_parametric_bare_splines_ctr = 0; // We need it for Bézier curves splines Telemetry gvf_parametric_bare_con gvf_parametric_bare_control; @@ -58,7 +57,7 @@ static void send_gvf_parametric_bare(struct transport_tx *trans, struct link_dev uint8_t traj_type = (uint8_t)gvf_parametric_bare_trajectory.type; uint32_t now = get_sys_time_msec(); - uint32_t delta_T = now - gvf_parametric_bare_t0; + uint32_t delta_T = now - gvf_c_t0; float wb = gvf_parametric_bare_control.w * gvf_parametric_bare_control.beta; @@ -73,7 +72,7 @@ static void send_gvf_parametric_bare(struct transport_tx *trans, struct link_dev static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev) { uint32_t now = get_sys_time_msec(); - uint32_t delta_T = now - gvf_parametric_bare_t0; + uint32_t delta_T = now - gvf_c_t0; /* if (delta_T < 200) @@ -115,34 +114,15 @@ void gvf_parametric_bare_control_2D(float kx, float ky, float f1, float f2, floa { uint32_t now = get_sys_time_msec(); - gvf_parametric_bare_control.delta_T = now - gvf_parametric_bare_t0; - gvf_parametric_bare_t0 = now; + gvf_parametric_bare_control.delta_T = now - gvf_c_t0; + gvf_c_t0 = now; - /* If the vehicle does not need to stop at the wp */ - if(!gvf_c_stopwp.stop_at_wp) + // We need at least two iterations for Delta_T + if (gvf_parametric_bare_control.delta_T > 300) { - // We need at least two iterations for Delta_T - if ((gvf_parametric_bare_control.delta_T > 300)) - { - /* Reset w since we assume the algorithm starts, because there cannot be any - * physical stop of 300 ms */ - gvf_parametric_bare_control.w = 0; - return; - } - } - else - { - /* TODO: Replace magic number for conversion from ms to s and the number - * of seconds of error */ - /* If the vehicle has to stop at any waypoint wp, it shall wait outside - * gvf_parametric_bare, gvf_c_stopwp.wait_time seconds, so when coming back - * into this function, delta_T could be really big. If that's the case then - * fix delta_T to a value, so the integration step is properly carried. - */ - if((gvf_parametric_bare_control.delta_T >= (gvf_c_stopwp.wait_time - 1)* 1000)) - { - gvf_parametric_bare_control.delta_T = 1.0 / PERIODIC_FREQUENCY; - } + /* Reset w since we assume the algorithm starts */ + gvf_parametric_bare_control.w = 0; + return; } // Carrot position diff --git a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h index 60bbb1166fc..395d6b5a6b1 100644 --- a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h +++ b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h @@ -69,6 +69,8 @@ * @param s Defines the direction to be tracked. It takes the values -1 or 1. * @param k_roll Gain for tuning the coordinated turn. * @param k_climb Gain for tuning the climbing setting point. +* @param k_psi Gain for tuning the heading setting point +* @param beta Variable to scale down w */ typedef struct { float w;