Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed error of a big integration step when vehicle stopped in gvf_par… #4

Open
wants to merge 2 commits into
base: devel_rover_stop
Choose a base branch
from
Open

Fixed error of a big integration step when vehicle stopped in gvf_par… #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7459406
Fixed error of a big integration step when vehicle stopped in gvf_par…
alfredoFBW Jul 17, 2024
20ac959
Added global variable for gvf cpu time computation according to revie…
alfredoFBW Jul 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Esta actualización no puede ser en increase_bz_pointer(), dado que se le llama una sola vez en el bloque de navegación del flight_plan.xml <call_once fun=" increase_bz_pointer()"/>. Tiene que ser aquí pues es donde se ejecuta.

return true;
}

Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/guidance/gvf_common.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
3 changes: 3 additions & 0 deletions sw/airborne/modules/guidance/gvf_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Añadida variable global de tiempo cpu actual para llamadas gvf (actualmente solo usado para paramétrico)


#endif // GVF_COMMON_H

18 changes: 10 additions & 8 deletions sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Importante, si no no se dibujan las curvas


float wb = gvf_parametric_bare_control.w * gvf_parametric_bare_control.beta;

Expand All @@ -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)
Expand Down Expand Up @@ -115,11 +114,14 @@ 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;

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
gvf_parametric_bare_control.delta_T = now - gvf_c_t0;
gvf_c_t0 = now;

// 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 */
gvf_parametric_bare_control.w = 0;
return;
}

Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down