Hello, guys,
I’m trying to write a self-defined trajectory and utilize the guided mode to let the quads track this trajectory by writing a 2-HZ loop such that each time a new waypoint along this trajectory can be updated. However, I found that the quads cannot track the desired trajectory very well. I got a log to compare the designed position, the desired position (the _pos_target variable in the AC_WPNav class), and the actual position. They are shown as follow. In this image, I just compare the three valuse in the X direction. I use the DAccX variable to denote the designed X position. In this case, I design a sine wave trajectory for the X direction. However, it is seen that the desired position (DPosX) has a large phase lag (means a long time delay). The actual position (PosX) tracks the desired position well, but also has a large phase lag with respect to the designed position.
So it can be concluded that the controller works well (as PosX tracks DPosX well), but the waypoint command generator has a large phase lag. I dig the code and realise that it may be due to the advance_wp_target_along_track function (called by the update_wpnav). I think it puts too much constraints on the intermediate waypoint. Is there any suggestion to improve the waypoint controller (the update_wanav and advance_wp_target_along_track functions) to avoid the target position delay and yield a fast waypoint response?