Fix to first order altitude hold gradient calculation #11063
Conversation
almaaro
changed the title
|
FYI - I've been wanting to get this logic out of navigator and into the FW position controller. MC is already ignoring it. #8883 |
|
This looks good, I just want to be sure Any thoughts on simply moving the entire FOH logic to the FW position controller? It removes most of the special checks and makes it usable for non-mission navigation states like RTL. |
| /* Save the distance between the current sp and the previous one */ | ||
| if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { | ||
| /* Save the distance between the current sp and our current position */ | ||
| if (pos_sp_triplet->current.valid) { | ||
|
|
||
| _distance_current_previous = get_distance_to_next_waypoint( |
There was a problem hiding this comment.
Drop "previous" from _distance_current_previous to reflect the change.
There was a problem hiding this comment.
Do you think _altitude_foh_distance would be good? I think it would describe its use better.
The altitudes of the current and previous waypoint and distance could be recorded in position_setpoint_triplet_poll() whenever the position setpoint triplet is updated? After that, the _pos_sp_triplet.current's altitude could be adjusted in control_position(...) (the original altitude would ofc be saved) |
|
Seems like an okay idea. Could help in some situations. |
|
Please reopen. See #11099 (comment) |
Test data / coverage
After pr: The altitude setpoint (black) is performing the first order hold seemingly well. (There are some weird spikes here and there, don't know why. Any ideas? However, they don't seem to affect the tecs altitude setpoint.) Tested with at least 20 flight.
Before pr: the altitude setpoint will change transiently if the current waypoint is within the acceptance radius of the next one.
Describe problem solved by the proposed pull request
Before pr: Upon arriving to point A (which is within the acceptance radius of waypoint 1) we calculate the altitude gradient between waypoints 1 and 2. However, if waypoint 1 is within the acceptance radius of waypoint 2, the altitude setpoint will change transiently.
After pr: The altitude control will follow the red line between the actual starting point of the descent (A) and C, which is at the start of waypoint 2 acceptance radius.
Describe your preferred solution
Instead of using the lat, lon and altitude of the previous waypoint (in this case 1), use the lat, lon and altitude of the vehicle at the beginning of the altitude change.