Also split a few functions into smaller ones for readability, fix
formatting, use geters to get the current state of the trajectory
instead of return arguments.
This helps when the current velocity is smaller than the target but that
the acceleration is too large such that the velocity will overshoot.
Without this check, the algorithm increases the acceleration which leads
to an even larger overshoot.
This solves many numerical issues when the trajectory is close to the
primary NE axes (small velocities). It is also more robust when dt is
large and has some jitter.
- ecl in PX4/Firmware (da6970329175df98ff46b2d9ddfa467ba438bb2e): 62fa464e4d
- ecl current upstream: 3b32ee4166
- Changes: 62fa464e4d...3b32ee4166
3b32ee4 2019-09-02 bresch - Flow aiding - Reset state when flow is enabled only if it is the only position/velocity aiding sensor. Until now, it was alway resetting if the vehicle does not have gps or external vision. This caused a reset/glitch at every stop (when range data gets valid)
be368f3 2019-08-28 kamilritz - Update comment
c5abfe6 2019-08-28 kamilritz - remove canonicalize and adapt comments
67512d8 2019-08-22 kamilritz - Update matrix library for CI
a2ff415 2019-08-22 kamilritz - Fix get frame aligning quaternion function
53eac6e 2019-08-21 kamilritz - Canonicalize alignment quaternion
933c32c 2019-08-20 kamilritz - Enable local frame alignment also without using it
ea352a6 2019-08-20 kamilritz - Dont use mag suffix for magnitude
05196db 2019-08-20 kamilritz - Fix alignment of local frame
The yaw pointing towards the center makes sense since that's the approach
direction anyways. But with the yaw feed forward results in a weird looking
bias when not orbiting yet.
The condition that the vehicle is more than 3m away from the circle line
was too sloppy. That often happens when the radius is changed by sticks.
A reapproach is only necessary when the center is moved and that's only
possible through the orbit command.
The initial approach to the circle to orbit on was very agressive since
it was just the controller trying to stay on the circle reaching the
limits. Now there's first an approach phase in which the vehicle reaches
the circle trajeectory in a smooth perpendicular line before starting the
orbit execution.
Before it was:
- not used anywhere
- copied from an old mission implementation version
- didn't plan in advance
- had a lot of broken cases
- dependent on a lot of parameters
I'm starting with a new relatively simple implementation that works as
expected for a minimum viable implementation and can be improved over time.
The first version is used to approach the circle path in Orbit mode to
verify the interface and get testing such that it gets eventually used
everywhere.
Script to update include paths:
for i in $(grep -rl 'include <px4_work_queue' src platforms); do sed -i 's/#include <px4_work_queue/#include <px4_platform_common\/px4_work_queue/' $i; done
- matrix in PX4/Firmware (341c0ae739753db98dcd0711e91f621c1666b16d): 56b069956d
- matrix current upstream: cc084e0791
- Changes: 56b069956d...cc084e0791
cc084e0 2019-08-26 Martina Rivizzigno - matrix: add method to check all values are nan (#82)
84b3da2 2019-08-22 kritz - Canonical Quaternion with tests (#81)
This changes two things:
- This adds "-beta" as a valid tag suffix.
- This changes "rc" without "-" to "-rc". "rc" without is now described
as deprecated and with "-rc" is preferred.
It was broken because _velocity_setpoint is used for input and output
and was assumed to be the input at a place where it was already overwitten
To clarify this, the input setpoint is renamed "target"
