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.

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

Geodesic grid and compass calibration completion mask #3981

Closed
wants to merge 25 commits into
base: master
from

Conversation

Projects
None yet
3 participants
@guludo
Contributor

guludo commented Apr 25, 2016

Hi all,

This PR addresses #3724 in the sense that it fills the completion mask in the compass calibration progress MAVLink message.

The bitmask is modified in the following situations:

  • On each accepted sample, find the section it crosses and set the corresponding bit.
  • After thinning the samples (which is done right before it enters step two), reset and update the bitmask for the remainder.
  • Every time the calibration parameters are changed (which happens with a successful sphere of ellipsoid fit), reset and update the bitmask.

I've implemented it that way so that listeners always get what is closest to "reality".

In px4-v2, this adds about 1.6k of flash size, from which about 750 bytes is static data, and 0 of stack.

@tridge, I've tried your suggestion here. It didn't passed the tests and I believe the reason for that is that the subtriangles aren't all equilateral when projected to a sphere.

With respect to data usage, considering sizeof(float) as 4 bytes, @tridge's suggestion would use 480 bytes for the normalized centroid vectors (only 40 vectors would be enough instead of 80).

Best regards,
Gustavo Sousa

dgrat and others added some commits Feb 25, 2016

AP_Math: Replaces is_zero with a template function
This function makes only sense for floating point types. However, in AP this
function was also used for ints. I corrected this.
AP_Math: Matrix3: add det() function
Function to calculate determinant. Additionally, add unit tests.
tests: add macro for printing test parameter
Google Test allows to instantiate tests for a list of different values, which
are called parameters. A common use of that feature in Ardupilot will be that a
parameter will be represented by an object that will have the value to be
tested and information about that value. That information will basically map
the expected behavior of tests on the value stored by the parameter.

The macro added in this patch allows to easily print the value of a failed
test's parameter.
AP_Math: AP_GeodesicGrid: add first implementation
The search for the geodesic section is still naive in this version.
Additionally, add unit tests.
AP_Math: benchmark_geodesic_grid: add benchmark
That will help to benchmark improvements on the section search algorithm.
AP_Math: AP_GeodesicGrid: change order of triangles
 - Change the order of the icosahedron triangles so that there's a uniform way of
finding the opposite triangle. The order visually still makes sense.

 - Change test to accommodate the order change.
AP_Math: test_geodesic_grid: test triangles indexes
That helps figuring out which intermediate step failed the high level call.
AP_Math: AP_GeodesicGrid: optimize with neighbor umbrellas
This is a first optimization of the algorithm. The struct for the neighbor
umbrella has only one member, but new members will be added in the next
optimization.

The table below summarizes the mean CPU time in ns from the brenchmark results
on an Intel(R) Core(TM) i7-3520M CPU @ 2.90GHz processor:

Cases  | Naive implementation | First Optimization
--------------------------------------------------
Min.   |                 26.0 |              28.00
1st Qu.|                 78.0 |              48.75
Median |                132.0 |              57.00
Mean   |                130.1 |              61.20
3rd Qu.|                182.2 |              76.00
Max.   |                234.0 |              98.00

This optimization reduces the mean time for the worst case (Max. line) by more
than 50%.
AP_Math: AP_GeodesicGrid: optimize _from_neighbor_umbrella()
This is the second optimization. With that we don't have to iterate over the
umbrella's components.

The table below summarizes the mean CPU time in ns from the brenchmark results
on an Intel(R) Core(TM) i7-3520M CPU @ 2.90GHz processor:

       | Naive implementation | First Optimization | Second Optimization
------------------------------------------------------------------------
Min.   |                 26.0 |              28.00 |                26.0
1st Qu.|                 78.0 |              48.75 |                39.0
Median |                132.0 |              57.00 |                41.0
Mean   |                130.1 |              61.20 |                41.6
3rd Qu.|                182.2 |              76.00 |                47.0
Max.   |                234.0 |              98.00 |                54.0
AP_Math: AP_GeodesicGrid: fix algorithm for null vector
If v is the null vector, then alpha * v is still the null vector for any alpha
as a real number. That means that the null vector doesn't cross any section.
AP_Math: AP_GeodesicGrid: remove section_triangle() function
That function was only being used by the unit tests and the benchmark. In order
to remove memory usage, the triangles will be removed, since we don't actually
need to keep them in real situations. Thus, this patch removes that function
and copy those triangles to the test and benchmark.
AP_Math: AP_GeodesicGrid: remove triangles
There's no need to keep those triangles, since we just need the inverses.
AP_Math: AP_GeodesicGrid: reduce number of inverses by half
We don't actually need all of them, since the second half is for the opposite
triangles. In that case we just need to negate the resulting vector when
changing basis.
AP_Math: AP_GeodesicGrid: use uint8_t for _neighbor_umbrellas
All integers there are limited to the range [0,20), so uint8_t is enough.
AP_Math: AP_GeodesicGrid: reduce number of _neighbor_umbrellas items
Only the first half is necessary. The values for the other half can be derived.
AP_Math: AP_GeodesicGrid: make data static
That gives the change of storing that data in flash storage in some
architectures. That doesn't happen yet though, some extra changes are required
for that to happen.
AP_Math: make vectors and matrix constructors constexpr
That allows some object to be constructed at compile time.
AP_Compass: implement completion mask
Fill the completion mask and send that through MAVLink while calibrating the
compass.
AP_Math: add geodesic_grid toolset
That was used to aid development AP_GeodesicGrid and understanding its
concepts.

@guludo guludo referenced this pull request Apr 25, 2016

Closed

RFC: AP_GeodesicGrid #3926

@lucasdemarchi

This comment has been minimized.

Show comment
Hide comment
@lucasdemarchi

lucasdemarchi May 16, 2016

Contributor

@guludo take a look on https://github.com/ArduPilot/ardupilot/commits/pr/geodesic-grid-v2

Some comments:

  • Coding style with regard the & for references and switch (fixed)
  • No need for const if the argument is bool (fixed)
  • Year in the copyright should be 2016, not 2015-2016
  • We don't use inclusive==false in the CompassCalibrator, why do we bother with it? I'd rather choose one and document it instead of supporting dead code
Contributor

lucasdemarchi commented May 16, 2016

@guludo take a look on https://github.com/ArduPilot/ardupilot/commits/pr/geodesic-grid-v2

Some comments:

  • Coding style with regard the & for references and switch (fixed)
  • No need for const if the argument is bool (fixed)
  • Year in the copyright should be 2016, not 2015-2016
  • We don't use inclusive==false in the CompassCalibrator, why do we bother with it? I'd rather choose one and document it instead of supporting dead code
@lucasdemarchi

This comment has been minimized.

Show comment
Hide comment
@lucasdemarchi

lucasdemarchi May 16, 2016

Contributor

Applied.

Contributor

lucasdemarchi commented May 16, 2016

Applied.

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