- gyro filtering (low-pass and notch) only performed on primary gyro in `sensors/vehicle_angular_velocity` instead of every gyro in `PX4Gyroscope`
- sample rate is calculated from actual updates (the fixed value was slightly wrong in many cases, and very wrong in a few)
- In the FIFO case the array is now averaged and published in `sensor_gyro` for filtering downstream. I'll update this in the future to use the full FIFO array (if available), but right now it should be fine.
* NotchFilter: add NotchFilter template and test for float type
* NotchFilterTest: add test for Vector3f notch filter
* PX4Gyroscope: add notch filter with IMU_GYRO_NF_(FREQ|BW) parameters to
set the notch center frequency and bandwidth
* Use position instead of last setpoint
This calculates the target velocities better taking into account disturbances along
the flight route. Previously entry angles and more were calculated assuming the flight path
originates directly from the direction of the previous waypoint. This corrects this assumption
to instead make the direction come from the vehicle location.
* Allow to specify a final speed given a braking distance.
This is to allow planning to not stop at a waypoint, but instead
to reach the waypoint while maintaining a certain velocity
* Updated src/lib/matrix
* Account for speed at target when determining constraints
* Separate constraints into x/y components
* Use setpoint position, not vehicle position
* Fix whitespace, add documentation
and remove the px4_ prefix, except for px4_config.h.
command to update includes:
for k in app.h atomic.h cli.h console_buffer.h defines.h getopt.h i2c.h init.h log.h micro_hal.h module.h module_params.h param.h param_macros.h posix.h sem.h sem.hpp shmem.h shutdown.h tasks.h time.h workqueue.h; do for i in $(grep -rl 'include <px4_'$k src platforms boards); do sed -i 's/#include <px4_'$k'/#include <px4_platform_common\/'$k/ $i; done; done
for in $(grep -rl 'include <px4_config.h' src platforms boards); do sed -i 's/#include <px4_config.h/#include <px4_platform_common\/px4_config.h'/ $i; done
Transitional headers for submodules are added (px4_{defines,log,time}.h)
- Delete left over identity matrix.
- Corner case with a zero element when using the signum function:
We always need a sign also for zero.
- Corner case with arbitrary yaw but and 180 degree roll or pitch:
Reduced attitude control calculation always rotates around roll
because there's no right choice when neglecting yaw. In that small
corner case it's better to just use full attitude contol and hence
rotate around the most efficient roll/pitch combination.
to prevent a setpoint step when moving the stick over the border of the deadzone
and to enable very small inputs even from a control stick that needs a big deadzone
In this particular case it does no harm,
but since in other cases it can lead to
problems I didn't want to add an exception
for this case to fix_headers.sh, that currently
chokes on this because it doesn't know better
than that it's a bad thing.
Note on how #pragma once works: when encountered
(aka the #ifdef that it is inside has to
be true), the compiler marks the whole
file as "seen" (this is implementation
defined, but most implementations store
the inode of the file). Subsequent #include's
of that file/inode are then completely skipped.
Hence it doesn't matter if the #pragma is at
the beginning, at the end or in the middle,
but it should be encountered every #include,
usually, and thus not be inside an #if... #endif
construct.
These headers files were missing from the header files that
I added them to; the fact that they were missing didn't
lead to compile errors because by coincidence the missing
headers are included in the source files before including
these headers. But, after the reordering of header inclusions
by Tools/fix_headers.sh, these cases will give rise to compiler
errors.
This patch reorders px4_defines.h to make it more readable (I think)
but more importantly, cleans up the #include <math.h>/<cmath>
and [std::]isfinite stuff.
My main goal was to completely get rid of including math.h/cmath,
because that doesn't really belong in a header that is supposed to
define macro's and is included in almost every source file (if not
all).
I'm not sure what it did before ;) (pun intended), but now it does
the following:
PX4_ISFINITE is only used in C++ code (that was already the case,
but hereby is official; for C code just use 'isfinite()') and is
defined to be std::isfinite, except on __PX4_QURT because that uses
the HEXAGON toolset which (erroneously) defines isfinite as macro.
I would have liked to remove PX4_ISFINITE completely from the code
and just use std::isfinite whereever that is needed, but that would
have required changing the libecl submodule, and at the moment I'm
getting tired of changing submodules... so maybe something for the
future.
Also, all includes of <math.h> or <cmath> have been removed except
for __PX4_NUTTX. Like the HEXAGON toolset NuttX currently defines
isfinite as macro for C++. So, we could have solved this in the
same was as __P4_QURT; but since we can fix NuttX ourselves I chose
to add a kludge to px4_defines.h instead that fixes this problem,
until the time that NuttX can be fixed (again postponing changing
a submodule). The kludge still demands including <cmath>, thus.
After removal of the math header file, it needed to be included
in source files that actually need it, of course.
Finally, I had a look at the math macro's (like M_PI, M_PI_F,
M_DEG_TO_RAD etc). These are sometimes (erroneously) defined in
certain math.h header files (like both, hexagon and nuttx).
This is incorrect: neither the C nor the C++ standard defines
math constants (neither as macro nor otherwise). The "problem"
here was that px4_defines.h defined some of the M_*_F float
constants in terms of the M_* double constant, which are
sometimes not defined either thus. So, I cleaned this up by
defining the M_*_F math constants as float literals in px4_defines.h,
except when they are defined in math.h for that platform.
This means that math.h has to be always included when using those
constants, but well; not much difference there as those files
usually also need/use the macro NAN (which *is* a standard macro
defined by math.h).
Finally finally, DEFAULT_PARAM_FILE was removed as it isn't
used anymore.
All in all I think the resulting px4_defines.h is nice, giving me
much less the feeling of a nearly unmaintainable and over time
slowly growing collection of kludges and hacks.
It makes more sense to set the optimization flags on a platform basis
instead of individually for each module. This allows for different
optimization options for SITL, NuttX, Snapdragon, etc.
This was horribly wrong. Matrix is first cast into a matrix of size NxM (which is supposed to be the size of the result - NOT the starting point) so the transpose result becomes garbage. Instead make "Me" an MxN matrix as the original. Took me a whole evening to figure out this problem. Now my Kalman filter finally returns good results.
