Commit Graph

376 Commits

Author SHA1 Message Date
dgrat 7ff8004f8f AP_Math: Replace is_zero() with a template function
This function only makes sense for floating point types. However this
function was also used for ints.
2016-05-16 19:08:35 -03:00
Ricardo de Almeida Gonzaga 481e3a2af6 AP_Math: Fix typos 2016-05-13 19:20:06 -03:00
Lucas De Marchi aa974399d0 AP_Math: use inline wrappers for constrain_* functions
This avoids some warnings about "constrain_float defined but not used"
in some compilers.
2016-05-11 22:51:19 -03:00
Lucas De Marchi bd6e268122 AP_Math: fix coding style
- cleanup whitespace
  - function reorder
  - fix brace position
2016-05-10 11:41:26 -03:00
Lucas De Marchi 1dbffef7ea AP_Math: remove trailing whitespace on headers 2016-05-10 11:41:26 -03:00
Lucas De Marchi ab1fa4b435 AP_Math: reorganize headers
Sort and add pragma once where needed.
2016-05-10 11:41:26 -03:00
dgrat 6d3b491c02 AP_Math: Replace is_equal with a type safe template function
It makes sense to consider also other floating point types.
2016-05-10 11:41:26 -03:00
dgrat 503867b7dc AP_Math: Replace safe_sqrt() by template function 2016-05-10 11:41:26 -03:00
dgrat 5deb0e8e03 AP_Math: Replace safe_asin() by template function 2016-05-10 11:41:26 -03:00
Lucas De Marchi 846b4927ec AP_Math: use if/else chain instead of 2 ternary operators 2016-05-10 11:41:26 -03:00
dgrat 174f899a29 AP_Math: Replace the constrain_* functions by a single template
Besides being simpler this reduces ~4k in the binary size for PX4.
2016-05-10 11:41:26 -03:00
Lucas De Marchi 348b07609c AP_Math: remove macros from unit tests
Avoid warnings like:

[2130/2168] Compiling libraries/AP_Math/tests/test_math.cpp
../../libraries/AP_Math/tests/test_math.cpp: In member function ‘virtual void MathTest_IsZero_Test::TestBody()’:
../../libraries/AP_Math/tests/test_math.cpp:73:196: warning: converting ‘false’ to pointer type for argument 1 of ‘char
testing::internal::IsNullLiteralHelper(testing::internal::Secret*)’ [-Wconversion-null]
../../libraries/AP_Math/tests/test_math.cpp:74:199: warning: converting ‘false’ to pointer type for argument 1 of ‘char
testing::internal::IsNullLiteralHelper(testing::internal::Secret*)’ [-Wconversion-null]

Use EXPECT_TRUE() and EXPECT_FALSE() from gtest instead.
2016-05-10 11:41:26 -03:00
dgrat 41661f815f AP_Math: Replace the pythagorous* functions with a variadic template
The new function can deal with a variable number of function parameters.
Additionally, I renamed the functions to norm(), because this is the
standard name used in several other projects.
2016-05-10 11:41:26 -03:00
Lucas De Marchi 880f130670 AP_Math: fix loss of precision on float addition
When using wrap_180_cd() we are adding a small float (180 * 100) to a
possibly big number. This may lose float precision as illustrated by the
unit test failing:

    OUT: ../../libraries/AP_Math/tests/test_math.cpp:195: Failure
    OUT: Value of: wrap_180_cd(-3600000000.f)
    OUT:   Actual: -80
    OUT: Expected: 0.f
    OUT: Which is: 0
2016-05-10 11:41:26 -03:00
dgrat 76362caee0 AP_Math: Replace wrap_* functions with template versions 2016-05-10 11:41:26 -03:00
dgrat 49cfd6fd9b AP_Math: Add gtest support for some functions
These functions (or variants thereof) now have unit tests:

  - is_zero()
  - is_equal()
  - sq()
  - pythagorous()
  - constrain()
  - wrap_180()
  - wrap_360()

Some tests in wrap_180_cd are failing: -180 should be wrapped to 180,
not -180.
2016-05-10 11:41:26 -03:00
Lucas De Marchi ceea7540fd AP_Math: rename test file
This will be reused for more math tests.
2016-05-10 11:41:26 -03:00
Gustavo Jose de Sousa b594b5a08b AP_Math: matrix_alg: disable FE_OVERFLOW in inverse4x4() for SITL
There are occasional overflows on the determinant calculation in inverse4x4()
when using calibration SITL model.
2016-05-10 16:16:37 +10:00
Gustavo Jose de Sousa a7543d369f AP_Math: matrix_alg: protect inverseixi() against overflow
Fail on inverse3x3() and inverse4x4() if there's float overflow during the
determinant calculation.
2016-05-10 16:16:37 +10:00
Gustavo Jose de Sousa 39f72610e2 AP_Math: quaternion: fix style
Remove trailing spaces and run astyle.
2016-05-10 16:16:37 +10:00
Andrew Tridgell e8142b0b5b AP_Math: added wrap_360() 2016-05-07 18:27:21 +10:00
Andrew Tridgell 97d27ce58f AP_Math: fixed memory leak
found by coverity
2016-04-26 16:41:44 +10:00
Andrew Tridgell 6165c42535 AP_Math: added from_axis_angle() method on Matrix3f
for arbitrary rotations in simulator
2016-04-22 10:28:15 +10:00
Michael du Breuil bb7cf6c0b6 AP_Math: Update location_sanitize to sanitize for lat/lng 2016-04-17 19:00:03 -07:00
Ricardo de Almeida Gonzaga 5bd034a5a8 Global: start using cmath instead of math.h 2016-04-05 21:06:19 -07:00
Andrew Tridgell 6bff07397e AP_Math: added linear_interpolate() function 2016-04-02 22:44:47 +11:00
Lucas De Marchi b4a8a0b961 AP_Math: replace header guard with pragma once 2016-03-16 18:40:41 +11:00
Gustavo Jose de Sousa 6729475b25 AP_Math: fix eulers example for PX4 2016-03-14 11:54:31 -03:00
Gustavo Jose de Sousa d9bb696fa3 AP_Math: avoid bias in rand_num() in eulers example
Use RAND_MAX to avoid bias.
2016-03-14 11:54:31 -03:00
Tom Pittenger 278fb2e60d AP_Math: add location sanity checker/fixer util 2016-03-02 08:48:26 -08:00
dgrat 672acdc8ef AP_Math: Created location.h header for location functions
Helps to order AP_Math functions by purpose.
2016-02-27 02:51:33 -03:00
Lucas De Marchi 7f685a12bd AP_Math: remove trailing whitespaces and tabs 2016-02-27 02:51:33 -03:00
dgrat 5148e41c1a AP_Math: Cleaned macro definitions
Moved Definitions into a separate header. Replaced PI with M_PI and
removed the M_PI_*_F macros.
2016-02-27 02:51:33 -03:00
dgrat 7c4c8ea579 AP_Math: Remove ROTATION_COMBINATION_SUPPORT
This function is not used.
2016-02-27 02:51:33 -03:00
Daniel Frenzel ead51a9d19 AP_Math: Removed useless "undef INLINE" 2016-02-25 02:10:39 -03:00
Daniel Frenzel ef7cf7c4aa AP_Math: Removed useless header
"float.h" does not exist. It is useless and wrong to include it.
2016-02-25 02:10:39 -03:00
Peter Barker 80bc7a50d7 AP_Math: define MATH_CHECK_INDEXES
Wrapped in ifndefs so the top-level Makefile can override

Assume MATH_CHECK_INDEXES is always defined
2016-02-19 12:34:23 -02:00
Gustavo Jose de Sousa c2e3f05dbf waf: ardupilotwaf: prefix build context methods with ap_
It helps to distinguish between things from waf and things from ardupilotwaf.
2016-01-22 20:10:29 -02:00
Gustavo Jose de Sousa a2d2661765 waf: use methods from bld instead of ardupilotwaf for the remaining 2016-01-22 20:10:29 -02:00
Gustavo Jose de Sousa 3d22490397 waf: examples: use methods from bld instead of ardupilotwaf 2016-01-22 20:10:29 -02:00
Gustavo Jose de Sousa d281067bcc waf: make example binaries be placed in 'examples' dir
This commit makes examples' wscripts use ardupilotwaf.example() instead of
ardupilot.program().
2016-01-15 16:46:41 -02:00
Andrew Tridgell 60af7a6087 AP_Math: removed matrix3 parameter support 2016-01-04 11:14:43 +11:00
Andrew Tridgell 16e0a6d7b0 AP_Math: fixed angle between two vector3s 2016-01-01 12:41:05 +11:00
Lucas De Marchi 180359d6dd AP_Math: fix unit test
For ROTATION_ROLL_90_PITCH_68_YAW_293 consider the angles as 90, 68.8
and 293.3 degrees to pre-calculate rotation. This matches the rotation
matrix used in code.

While at it, check not only the values are close enough but also the
length of the vector.
2015-12-30 20:22:28 -02:00
Lucas De Marchi ba3325ffd3 AP_Math: rename rotation
The rotations are supposed to follow the name of the enum, in order. The
ROTATION_YAW_293_PITCH_68_ROLL_90 was added with the name of an
intrinsic 321 rotation, but the matrix is actually a 123 rotation,
following the other rotations already present.

Change the name to follow the other names.
2015-12-30 20:22:27 -02:00
Jonathan Challinger 83d5a6664a AP_Math: minor changes to matrix_alg in response to review 2015-12-29 22:57:21 -08:00
bugobliterator 1a4b4fa85e AP_Math: add inverse matrix test to check if inverse(mat)*mat = I
where I is an identity matrix (a matrix with diagonal elements = 1)
2015-12-29 10:46:35 -08:00
bugobliterator fe62a049bd AP_Math: implement LU decomposition based matrix inverse
Replaces previous matlab generated code, which was giving imprecise results
2015-12-29 10:46:34 -08:00
Siddharth Bharat Purohit a0c3cbffee AP_Math: add inverse matrix test example
fix example build
2015-12-29 10:46:34 -08:00
Siddharth Bharat Purohit 5af0cc8eaf AP_Math: add inverse rotate example function 2015-12-29 10:46:34 -08:00
Jonathan Challinger 7ed8b3814f AP_Math: add rotate_inverse to Vector3 2015-12-29 10:46:34 -08:00
Lucas De Marchi a096703b06 Global: don't link with AP_Progmem
AP_Progmem is not used anymore.
2015-12-27 15:58:12 -02:00
Lucas De Marchi 502077d763 AP_Math: stop using Progmem.h 2015-12-27 15:58:12 -02:00
José Roberto de Souza d74bd533c8 AP_Math: Add function to convert frequency to/from microseconds 2015-12-16 08:18:17 +11:00
José Roberto de Souza 38575dd87a AP_Math: Fix parameter name in nsec_to_hz() 2015-12-16 08:18:17 +11:00
Jonathan Challinger 0a701088d4 AP_Math: fix bug in Quaternion::operator*= 2015-12-14 11:15:33 +09:00
Jonathan Challinger fff275fd99 AP_Math: add wrap_2PI 2015-12-09 19:58:25 +09:00
Tom Pittenger f8b0a6a977 AP_Math: compiler warnings - undeclared function 2015-12-07 16:05:00 +09:00
Tom Pittenger 103af93ec3 AP_Math: compiler warnings - float compare to constant (zero) 2015-12-07 16:04:59 +09:00
Lucas De Marchi f5c03c786d AP_Math: remove unused AP_ADC_AnalogSource 2015-12-03 13:32:43 +11:00
Gustavo Jose de Sousa eef784fbe6 AP_Math: add benchmark for matrix multiplication 2015-12-03 07:54:31 +11:00
Gustavo Jose de Sousa 4266e924d0 AP_Math: add unit test for vector rotations
This unit test already shows that rotation is wrong for
ROTATION_YAW_293_PITCH_68_ROLL_90.
2015-12-03 07:54:31 +11:00
Caio Marcelo de Oliveira Filho 6e7b73610d waf: add waf support 2015-12-03 07:54:30 +11:00
Lucas De Marchi aa9168e0e9 AP_Math: remove unused maxf() 2015-12-01 16:28:18 -02:00
Lucas De Marchi 6f88fba8e5 AP_Math: remove unused minf() 2015-12-01 16:28:18 -02:00
Lucas De Marchi e0a0514c79 AP_Math: turn MIN/MAX macros into inline functions
The problem with the current MIN/MAX macros is that they evaluate twice
the arguments. For example, these cases provide unintended results:

	// a is incremented twice
	a = MIN(a++, b);
	// foo() with side-effects
	a = MIN(foo(), b);

The alternative implementation here was provided by Daniel Frenzel using
template function. It doesn't have type safety as std::min and std::max,
but adding type safety would mean to check case by case what would be a
reasonable type and add proper casts. Here the arguments for MIN and MAX
can have different types and the return type is deduced from the
expression in the function.

Inspecting the current callers no place was found with the unintended
results above, but some in which now we don't calculate twice the
parameters will benefit from this new version. Examples:

	float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));

	float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch()

	accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f));

	track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);

	RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
2015-12-01 16:28:18 -02:00
Lucas De Marchi 2591261af6 Global: rename min and max macros to uppercase
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.

Changes generated with:

	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
2015-12-01 16:28:09 -02:00
Caio Marcelo de Oliveira Filho 8257e6ab89 AP_Math: examples use millis/micros/panic functions 2015-11-20 12:31:55 +09:00
Andrew Tridgell 8f4ce7f20b build: removed all nocore.inoflag files
these were APM2 specific
2015-11-16 08:05:17 +11:00
José Roberto de Souza d53911e3f6 AP_Math: Add functions to convert microseconds to/from nanoseconds 2015-11-10 17:05:34 +11:00
José Roberto de Souza ca533a9d94 AP_Math: Add functions to convert frequency to/from nanoseconds 2015-11-10 17:05:34 +11:00
Lucas De Marchi fb28f426da AP_Math: remove check for AVR CPUs
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
2015-11-04 12:14:14 +11:00
Lucas De Marchi c7871d1bca AP_Math: remove checks for HAL_BOARD_APM2 and HAL_BOARD_APM1 2015-11-04 12:14:12 +11:00
Lucas De Marchi 5244559010 Minimize AP_Progmem.h includes
Most of AP_Progmem is already gone so we can stop including it in most
of the places. The only places that need it are the ones using
pgm_read_*() APIs.

In some cases the header needed to be added in the .cpp since it was
removed from the .h to reduce scope. In those cases the headers were
also reordered.
2015-10-30 14:35:32 +09:00
Lucas De Marchi 20c6ffc5e3 Replace use of UARTDriver::printf_P() with UARTDriver::printf()
This also starts to show warnings on places that were already using
wrong printf format strings.
2015-10-30 14:35:25 +09:00
Lucas De Marchi 6f4904189b Replace use of println_P() with println() 2015-10-30 14:35:22 +09:00
Lucas De Marchi 831d8acca5 Remove use of PROGMEM
Now variables don't have to be declared with PROGMEM anymore, so remove
them. This was automated with:

    git grep -l -z PROGMEM | xargs -0 sed -i 's/ PROGMEM / /g'
    git grep -l -z PROGMEM | xargs -0 sed -i 's/PROGMEM//g'

The 2 commands were done so we don't leave behind spurious spaces.

AVR-specific places were not changed.
2015-10-30 14:35:16 +09:00
Lucas De Marchi 2c38e31c93 Remove use of PSTR
The PSTR is already define as a NOP for all supported platforms. It's
only needed for AVR so here we remove all the uses throughout the
codebase.

This was automated with a simple python script so it also converts
places which spans to multiple lines, removing the matching parentheses.

AVR-specific places were not changed.
2015-10-30 14:35:04 +09:00
Caio Marcelo de Oliveira Filho 5020db4481 AP_Math: remove unnecessary includes
Remove unnecessary includes, in particular the includes for specific
boards. The list of libraries for 'polygon' example was updated so that
the example compiles again.
2015-10-21 09:16:09 +11:00
Caio Marcelo de Oliveira Filho ec52df991c build: compile only the HAL files needed by the board
Instead of requiring every program to specify the HAL related modules,
let the build system do it (in practice everything we compiled depended
on HAL anyway). This allow including only the necessary files in the
compilation.
2015-10-21 09:16:07 +11:00
Caio Marcelo de Oliveira Filho 2e464a53c2 AP_HAL: make code not depend on concrete HAL implementations
The switching between different AP_HAL was happening by giving different
definitions of AP_HAL_BOARD_DRIVER, and the programs would use it to
instantiate.

A program or library code would have to explicitly include (and depend)
on the concrete implementation of the HAL, even when using it only via
interface.

The proposed change move this dependency to be link time. There is a
AP_HAL::get_HAL() function that is used by the client code. Each
implementation of HAL provides its own definition of this function,
returning the appropriate concrete instance.

Since this replaces the job of AP_HAL_BOARD_DRIVER, the definition was
removed.

The static variables for PX4 and VRBRAIN were named differently to avoid
shadowing the extern symbol 'hal'.
2015-10-21 09:16:07 +11:00
Andrew Tridgell c9ec8b3f67 AP_Math: added O3 optimisation to core math libraries
this costs some flash space but speeds things up considerably
2015-10-20 14:36:53 +11:00
Randy Mackay 6145794da2 AP_Math: remove duplicate RADIUS_OF_EARTH definition 2015-10-05 21:00:14 +09:00
Randy Mackay 39340e70f8 Math: add Vector2 is_zero method 2015-09-16 15:10:25 +09:00
Valmantas Palikša d4daf19151 AP_Math: Move simple math function implementations to header for better
compile time optimization

Functions like sq() are better moved to the header file as inline.
Compiler can then optimize these out when used in code, this saves cpu
cycles with stack push, pop during function calls.
2015-09-09 09:57:51 +10:00
Tom Pittenger 4771d19073 AP_Math: added locations_are_same(loc1,loc2) helper
returns true if lat and lng are the same, ignores alt and options
2015-09-08 17:05:54 +10:00
Siddharth Bharat Purohit 1ffbffa0e7 AP_Math: check if failed to allocate memory 2015-09-03 16:59:14 +10:00
Gustavo Jose de Sousa 46774136a6 AP_Compass: standardize inclusion of libaries headers
This commit changes the way libraries headers are included in source files:

 - If the header is in the same directory the source belongs to, so the
 notation '#include ""' is used with the path relative to the directory
 containing the source.

 - If the header is outside the directory containing the source, then we use
 the notation '#include <>' with the path relative to libraries folder.

Some of the advantages of such approach:

 - Only one search path for libraries headers.

 - OSs like Windows may have a better lookup time.
2015-09-03 16:59:13 +10:00
Jonathan Challinger 1c100498d4 AP_Math: use is_zero instead of defining our own epsilon 2015-09-03 16:59:13 +10:00
Siddharth Bharat Purohit b654b1c21b AP_Math: add inverse matrix to math library 2015-09-03 16:59:12 +10:00
Randy Mackay 46c652e42f Math: maxf and minf functions 2015-08-19 16:44:34 +09:00
Gustavo Jose de Sousa 4790371f9b AP_Math: standardize inclusion of libaries headers
This commit changes the way libraries headers are included in source files:

 - If the header is in the same directory the source belongs to, so the
 notation '#include ""' is used with the path relative to the directory
 containing the source.

 - If the header is outside the directory containing the source, then we use
 the notation '#include <>' with the path relative to libraries folder.

Some of the advantages of such approach:

 - Only one search path for libraries headers.

 - OSs like Windows may have a better lookup time.
2015-08-11 16:38:18 +10:00
Lucas De Marchi 0471de2342 AP_Math: use ARRAY_SIZE macro 2015-07-21 14:25:09 +09:00
Lucas De Marchi fb2eb262e7 AP_Math: use ARRAY_SIZE instead of defining another one 2015-07-21 14:24:49 +09:00
Andrew Tridgell 6009ae55b1 AP_Math: fixed build of eulers example sketch 2015-07-17 08:24:49 +10:00
Andrew Tridgell 8f41d97548 AP_Math: fixed build error on PX4
variable set but not used with recent longitude_scale change
2015-07-06 08:19:57 +10:00
Andrew Tridgell 099392d3ca AP_Math: don't optimise longitude_scale on faster CPUs
it causes problems with replay
2015-07-05 17:27:53 +10:00
Andrew Tridgell 3edac37929 AP_Math: removed unused include in example 2015-06-01 17:35:03 +10:00
Andrew Tridgell 4d040bbb94 AP_Math: convert example from .pde to .cpp 2015-06-01 17:07:04 +10:00
Andrew Tridgell 2b828a794e AP_Math: fixed example build 2015-06-01 16:36:54 +10:00