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

Jump to bottom

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

wants to merge 2 commits into from

Conversation

alfredoFBW
Copy link

…ametric_bare.
Closes #3

@alfredoFBW alfredoFBW added the bug Something isn't working label Jul 17, 2024
@alfredoFBW alfredoFBW requested review from juanjimenez and Liagar July 17, 2024 14:15
@alfredoFBW alfredoFBW self-assigned this Jul 17, 2024
@alfredoFBW alfredoFBW linked an issue Jul 17, 2024 that may be closed by this pull request
Fix bug regarding integration step when stopping at waypoints in gvf_parametric_bare #3
Closed
Liagar
Liagar reviewed Jul 18, 2024
View reviewed changes
Copy link

@Liagar Liagar left a comment

Choose a reason for hiding this comment

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

Estamos pensando dos cosas:

  1. Creemos que la línea 142 de tu gvf_parametric_bare.c no es necesaria, siempre que se esté trabajando en modo paradas en WP hay que recalcular el delta_T ¿no?
  2. Tal vez podría cambiarse la asignación del delta_T de 1s por algo más flexible. Por ejemeplo podríamos reescribir el tiempo gvf_parametric_bare_t0 (habria que ponerla como global) cuando nos salimos de una parada, por ejemplo en la función increase_bz_pointer de gvf.c. ¿Cómo lo ves?

@alfredoFBW
Copy link
Author

Estamos pensando dos cosas:

  1. Creemos que la línea 142 de tu gvf_parametric_bare.c no es necesaria, siempre que se esté trabajando en modo paradas en WP hay que recalcular el delta_T ¿no?
  • En cualquier modo que se trabaje, el delta_T se recalcula en la línea 118, gvf_parametric_bare_control.delta_T = now - gvf_parametric_bare_t0;, el problema es si se está en modo paradas gvf_c_stopwp.stop_at_wp = 1. Cuando se vuelva a gvf_paramétrico después de hacer la parada, el gvf_paramétric_t0 será muy atrasado (el tiempo que haya parado) respecto a now, lo que da un valor de paso de integración muy elevado. El if((gvf_parametric_bare_control.delta_T >= (gvf_c_stopwp.wait_time - 1)* 1000)), permite queque el step sea un periodo de muestreo pero solo cuando se ha venido de parar o no se llama gvf_paramétrico desde hace mucho.
  • Por eso pongo el TODO: Creo que sería interesante definir un intervalo: if((gvf_parametric_bare_control.delta_T >= (gvf_c_stopwp.wait_time - MARGEN)* 1000) && (gvf_parametric_bare_control.delta_T <= (gvf_c_stopwp.wait_time + MARGEN)* 1000)).
  1. Tal vez podría cambiarse la asignación del delta_T de 1s por algo más flexible. Por ejemeplo podríamos reescribir el tiempo gvf_parametric_bare_t0 (habria que ponerla como global) cuando nos salimos de una parada, por ejemplo en la función increase_bz_pointer de gvf.c. ¿Cómo lo ves?
  • Mediante este método la línea 142 no sería necesaria. El único problema es usar otra variable global, que a veces cuesta mucho depurar si algo va mal.

alfredoFBW
alfredoFBW commented Jul 18, 2024
View reviewed changes
Copy link
Author

@alfredoFBW alfredoFBW left a comment

Choose a reason for hiding this comment

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

Añadidos los cambios para introducir variable global.

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

sw/airborne/modules/guidance/gvf_common.h
@@ -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)

sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c
@@ -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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@Liagar Liagar Liagar left review comments

@juanjimenez juanjimenez Awaiting requested review from juanjimenez

Assignees

@alfredoFBW alfredoFBW

Labels
bug Something isn't working
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

Fix bug regarding integration step when stopping at waypoints in gvf_parametric_bare
3 participants
@alfredoFBW @juanjimenez @Liagar