mirror of
https://github.com/RobTillaart/Arduino.git
synced 2024-10-03 18:09:02 -04:00
0.2.0 runningAngle
This commit is contained in:
parent
cd358a3b2b
commit
434f8e1adb
@ -6,7 +6,7 @@ jobs:
|
|||||||
lint:
|
lint:
|
||||||
runs-on: ubuntu-latest
|
runs-on: ubuntu-latest
|
||||||
steps:
|
steps:
|
||||||
- uses: actions/checkout@v2
|
- uses: actions/checkout@v3
|
||||||
- uses: arduino/arduino-lint-action@v1
|
- uses: arduino/arduino-lint-action@v1
|
||||||
with:
|
with:
|
||||||
library-manager: update
|
library-manager: update
|
||||||
|
@ -8,7 +8,7 @@ jobs:
|
|||||||
runs-on: ubuntu-latest
|
runs-on: ubuntu-latest
|
||||||
|
|
||||||
steps:
|
steps:
|
||||||
- uses: actions/checkout@v2
|
- uses: actions/checkout@v3
|
||||||
- uses: ruby/setup-ruby@v1
|
- uses: ruby/setup-ruby@v1
|
||||||
with:
|
with:
|
||||||
ruby-version: 2.6
|
ruby-version: 2.6
|
||||||
|
@ -10,7 +10,7 @@ jobs:
|
|||||||
test:
|
test:
|
||||||
runs-on: ubuntu-latest
|
runs-on: ubuntu-latest
|
||||||
steps:
|
steps:
|
||||||
- uses: actions/checkout@v2
|
- uses: actions/checkout@v3
|
||||||
- name: json-syntax-check
|
- name: json-syntax-check
|
||||||
uses: limitusus/json-syntax-check@v1
|
uses: limitusus/json-syntax-check@v1
|
||||||
with:
|
with:
|
||||||
|
@ -6,17 +6,32 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/)
|
|||||||
and this project adheres to [Semantic Versioning](http://semver.org/).
|
and this project adheres to [Semantic Versioning](http://semver.org/).
|
||||||
|
|
||||||
|
|
||||||
|
## [0.2.0] - 2023-02-22
|
||||||
|
- add **void setMode0()** ==> -180..180
|
||||||
|
- add **void setMode1()** ==> 0..360
|
||||||
|
- add **uint8_t getMode()** return 0 or 1.
|
||||||
|
- add RA_DEFAULT_WEIGHT to **setWeight()**
|
||||||
|
- change return type of **setWeight()** to bool (return false if clipped).
|
||||||
|
- add RA_MIN_WEIGHT + RA_MAX_WEIGHT as constants
|
||||||
|
- add examples
|
||||||
|
- update readme.md
|
||||||
|
- move code from .h to .cpp
|
||||||
|
- update GitHub actions
|
||||||
|
- update license 2023
|
||||||
|
- minor edits
|
||||||
|
|
||||||
|
----
|
||||||
|
|
||||||
## [0.1.5] - 2022-11-23
|
## [0.1.5] - 2022-11-23
|
||||||
- add changelog.md
|
- add changelog.md
|
||||||
- add RP2040 to build-CI
|
- add RP2040 to build-CI
|
||||||
- minor edits
|
- minor edits
|
||||||
|
|
||||||
|
|
||||||
## [0.1.4] - 2022-05-29
|
## [0.1.4] - 2022-05-29
|
||||||
- add GRADIANS support
|
- add GRADIANS support
|
||||||
|
|
||||||
## [0.1.3] - 2021-12-28
|
## [0.1.3] - 2021-12-28
|
||||||
- update library.json, readme, license,
|
- update library.json, readme, license,
|
||||||
- minor edits
|
- minor edits
|
||||||
|
|
||||||
## [0.1.2] - 2021-05-27
|
## [0.1.2] - 2021-05-27
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
MIT License
|
MIT License
|
||||||
|
|
||||||
Copyright (c) 2020-2022 Rob Tillaart
|
Copyright (c) 2020-2023 Rob Tillaart
|
||||||
|
|
||||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
of this software and associated documentation files (the "Software"), to deal
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
@ -43,6 +43,16 @@ raised by Edgar Bonet.
|
|||||||
[an issue]: https://github.com/RobTillaart/AverageAngle/issues/1
|
[an issue]: https://github.com/RobTillaart/AverageAngle/issues/1
|
||||||
|
|
||||||
|
|
||||||
|
#### Related
|
||||||
|
|
||||||
|
- https://github.com/RobTillaart/Angle
|
||||||
|
- https://github.com/RobTillaart/AngleConvertor
|
||||||
|
- https://github.com/RobTillaart/AverageAngle
|
||||||
|
- https://github.com/RobTillaart/RunningAngle
|
||||||
|
- https://github.com/RobTillaart/RunningAverage
|
||||||
|
- https://github.com/RobTillaart/RunningMedian
|
||||||
|
|
||||||
|
|
||||||
## Smoothing coefficient
|
## Smoothing coefficient
|
||||||
|
|
||||||
The output of the filter is efficiently computed as a weighted average
|
The output of the filter is efficiently computed as a weighted average
|
||||||
@ -50,16 +60,27 @@ of the current input and the previous output:
|
|||||||
|
|
||||||
output = α × current\_input + (1 − α) × previous\_output
|
output = α × current\_input + (1 − α) × previous\_output
|
||||||
|
|
||||||
The smoothing coefficient, α, is the weight of the current input in the
|
The smoothing coefficient, α, is the weight of the current input in the average.
|
||||||
average. It is called “weight” within the library, and should be set to
|
It is called “weight” within the library, and should be set to a value between
|
||||||
a value between 0.001 and 1. The larger the weight, the weaker the
|
0.001 and 1. The larger the weight (closer to 1), the weaker the smoothing,
|
||||||
smoothing. A weight α = 1 provides no smoothing at all, as the
|
so noise will affect the average quite a bit.
|
||||||
filter's output is a just a copy of its input.
|
Closer to zero, new values only affect the average minimal.
|
||||||
|
- A weight α = 1 provides no smoothing at all, as the
|
||||||
|
filter's output will be a copy of the input.
|
||||||
|
- A weight α = 0 will return only the first value added.
|
||||||
|
Therefore the weight should be minimal 0.001.
|
||||||
|
|
||||||
The filter has a smoothing performance similar to a simple running
|
The filter has a smoothing performance similar to a simple running
|
||||||
average over N = 2/α − 1 samples. For example, α = 0.2 is similar to
|
average over N = 2/α − 1 samples. For example, α = 0.2 is similar to
|
||||||
averaging over the last 9 samples.
|
averaging over the last 9 samples.
|
||||||
|
|
||||||
|
It is important to do test runs to find the optimal coefficient (range)
|
||||||
|
for your application. One should be aware that the frequency of adding
|
||||||
|
new angles will affect the time to stabilize the output.
|
||||||
|
|
||||||
|
Note also that it is possible to change the weight run-time.
|
||||||
|
This can be needed e.g. if the accuracy of the input improves over time.
|
||||||
|
|
||||||
|
|
||||||
## Usage
|
## Usage
|
||||||
|
|
||||||
@ -69,15 +90,16 @@ First, create a filter as an instance of `runningAngle`:
|
|||||||
runningAngle my_filter(runningAngle::DEGREES);
|
runningAngle my_filter(runningAngle::DEGREES);
|
||||||
```
|
```
|
||||||
|
|
||||||
The parameter of the constructor should be either
|
The parameter of the constructor should be
|
||||||
`runningAngle::DEGREES` or `runningAngle::RADIANS`. It is optional and
|
`runningAngle::DEGREES`, `runningAngle::RADIANS` or `runningAngle::GRADIANS`.
|
||||||
defaults to degrees.
|
This parameter is optional and defaults to degrees.
|
||||||
|
|
||||||
Then, set the “weight” smoothing coefficient:
|
Then, set the “weight” smoothing coefficient:
|
||||||
|
|
||||||
```c++
|
```c++
|
||||||
my_filter.setWeight(0.2);
|
my_filter.setWeight(0.2);
|
||||||
```
|
```
|
||||||
|
Note: the weight defaults to 0.80 so no need to set
|
||||||
|
|
||||||
Finally, within the main sketch's loop, feed the raw angle readings to
|
Finally, within the main sketch's loop, feed the raw angle readings to
|
||||||
the filter's `add()` method:
|
the filter's `add()` method:
|
||||||
@ -87,16 +109,22 @@ float heading = get_a_compass_reading_somehow();
|
|||||||
float smoothed_heading = my_filter.add(heading);
|
float smoothed_heading = my_filter.add(heading);
|
||||||
```
|
```
|
||||||
|
|
||||||
The method returns the smoothed reading within ± 180° (i.e. ± π rad).
|
The method returns the smoothed reading within ± 180° (i.e. ± π radians) by default.
|
||||||
|
|
||||||
See the “examples” folder for a more complete example.
|
The returned value is easily mapped upon 0..360 by adding 360 degrees ( 2π )
|
||||||
|
to the average if the average < 0.
|
||||||
|
Other mappings including scaling are of course possible.
|
||||||
|
|
||||||
Degree character = ALT-0176
|
Note: Degree character ° = ALT-0176 (windows)
|
||||||
|
|
||||||
|
|
||||||
## Interface
|
## Interface
|
||||||
|
|
||||||
### AngleType
|
```cpp
|
||||||
|
#include "runningAngle.h"
|
||||||
|
```
|
||||||
|
|
||||||
|
#### AngleType
|
||||||
|
|
||||||
- **enum AngleType { DEGREES, RADIANS, GRADIANS }** used to get type math right.
|
- **enum AngleType { DEGREES, RADIANS, GRADIANS }** used to get type math right.
|
||||||
|
|
||||||
@ -105,38 +133,79 @@ A full circle is defined as:
|
|||||||
- RADIANS = 2 π = 6.283...
|
- RADIANS = 2 π = 6.283...
|
||||||
- GRADIANS = 400°
|
- GRADIANS = 400°
|
||||||
|
|
||||||
GRADIANS are sometimes called GON.
|
GRADIANS are sometimes called GON.
|
||||||
|
|
||||||
There also exists a type milli-radians which is effectively the
|
There also exists a type milli-radians which is effectively the
|
||||||
same as RADIANS \* 1000. It won't be supported.
|
same as RADIANS \* 1000. This won't be supported.
|
||||||
|
mRad and other angle-types can be converted here - https://github.com/RobTillaart/AngleConvertor
|
||||||
|
|
||||||
|
|
||||||
### runningAngle
|
#### runningAngle
|
||||||
|
|
||||||
- **runningAngle(AngleType type = DEGREES)** constructor, default to DEGREES
|
- **runningAngle(AngleType type = DEGREES)** constructor, default to DEGREES.
|
||||||
- **float add(float angle)** adds value using a certain weight,
|
- **float add(float angle)** adds a new value using a defined weight.
|
||||||
except the first value after a reset is used as initial value.
|
Except for the first value after a reset, which is used as initial value.
|
||||||
The **add()** function returns the new average.
|
The **add()** function returns the new average.
|
||||||
- **void reset()** resets the internal average and weight to start clean again.
|
- **void reset()** resets the internal average to 0 and weight to start "clean" again.
|
||||||
If needed one should call **setWeight()** again!
|
If needed one should call **setWeight()** again!
|
||||||
- **float getAverage()** returns the current average value.
|
- **float getAverage()** returns the current average value.
|
||||||
- **void setWeight(float weight)** sets the weight of the new added value.
|
- **void setWeight(float weight = RA_DEFAULT_WEIGHT)** sets the weight of the new added value.
|
||||||
Value will be constrained between 0.001 and 1.00
|
Value will be constrained between 0.001 and 1.0.
|
||||||
- **float getWeight()** returns the current set weight.
|
Default weight = RA_DEFAULT_WEIGHT == 0.80.
|
||||||
|
- **float getWeight()** returns the current / set weight.
|
||||||
- **AngleType type()** returns DEGREES, RADIANS or GRADIANS.
|
- **AngleType type()** returns DEGREES, RADIANS or GRADIANS.
|
||||||
- **float wrap(float angle)** wraps an angle to <-180..+180> <-PI..PI> <-200..200> depending on the type set.
|
- **float wrap(float angle)** wraps an angle to \[-180..+180> \[-PI..PI>
|
||||||
|
or \[-200..200> depending on the type set.
|
||||||
|
|
||||||
|
|
||||||
## Operation
|
#### Mode
|
||||||
|
|
||||||
See examples
|
- **void setMode0()** average interval = \[-180..180>
|
||||||
|
- **void setMode1()** average interval = \[0..360>
|
||||||
|
- **uint8_t getMode()** returns current mode = 0 or 1.
|
||||||
|
|
||||||
|
|
||||||
|
## Performance add()
|
||||||
|
|
||||||
|
Being the most important worker function, doing float math.
|
||||||
|
(based on time-add.ino on UNO)
|
||||||
|
|
||||||
|
| version | mode | CPU cycles | us per add | relative |
|
||||||
|
|:---------:|:------:|-------------:|-------------:|-----------:|
|
||||||
|
| 0.1.5 | 0 | 681 | 42.5625 us | 100% |
|
||||||
|
| 0.2.0 | 0 | 681 | 42.5625 us | 100% |
|
||||||
|
| 0.2.0 | 1 | 681 | 42.5625 us | 100% |
|
||||||
|
|
||||||
|
|
||||||
## Future
|
## Future
|
||||||
|
|
||||||
- get some numbers about the noise in the angles (stats on the delta?)
|
#### Must
|
||||||
- runtime change of type
|
|
||||||
- conversion
|
- improve documentation.
|
||||||
|
|
||||||
|
#### Should
|
||||||
|
|
||||||
|
- should **add()** return the average? (yes)
|
||||||
|
- or make a **fastAdd()** that doesn't?
|
||||||
|
|
||||||
|
#### Could
|
||||||
|
|
||||||
|
- add examples.
|
||||||
|
- compass HMC6352 lib or simulator.
|
||||||
|
- AS5600 angle measurement sensor
|
||||||
|
- update unit tests.
|
||||||
|
|
||||||
|
|
||||||
|
#### Wont
|
||||||
|
|
||||||
|
- get statistics about the noise in the angles (stats on the delta?).
|
||||||
|
- not the goal of this lib ==> use statistics library
|
||||||
|
- optimize **wrap()** to be generic => no loop per type.
|
||||||
|
- needs variables for -180 / 180 / 360 (RAM vs PROGMEM)
|
||||||
|
- derived class for degrees only? (max optimization)
|
||||||
|
- runtime change of type
|
||||||
|
- no, too specific scenario.
|
||||||
|
- conversion needed?
|
||||||
- add mixed types. 45° + 3 radians = ??
|
- add mixed types. 45° + 3 radians = ??
|
||||||
|
==> user can do this.
|
||||||
|
|
||||||
|
|
||||||
|
@ -118,5 +118,5 @@ void test3()
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// -- END OF FILE --
|
// -- END OF FILE --
|
||||||
|
|
||||||
|
@ -0,0 +1,54 @@
|
|||||||
|
//
|
||||||
|
// FILE: runningAngle_0_360.ino
|
||||||
|
// AUTHOR: Rob Tillaart
|
||||||
|
// PURPOSE: demo mapping average from -180..180 to 0..360
|
||||||
|
|
||||||
|
|
||||||
|
#include "runningAngle.h"
|
||||||
|
|
||||||
|
uint32_t start, stop;
|
||||||
|
|
||||||
|
|
||||||
|
runningAngle heading(runningAngle::DEGREES);
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200);
|
||||||
|
Serial.println(__FILE__);
|
||||||
|
|
||||||
|
start = millis();
|
||||||
|
for (int32_t a = 0; a < 36000; a++)
|
||||||
|
{
|
||||||
|
heading.reset();
|
||||||
|
float angle = a * 0.01;
|
||||||
|
heading.add(angle);
|
||||||
|
|
||||||
|
float average = heading.getAverage();
|
||||||
|
// map output 0..359.999
|
||||||
|
if (average < 0)
|
||||||
|
{
|
||||||
|
average += 360.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(angle - average) > 0.0001)
|
||||||
|
{
|
||||||
|
Serial.print(angle);
|
||||||
|
Serial.print("\t");
|
||||||
|
Serial.print(average);
|
||||||
|
Serial.println();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
stop = millis();
|
||||||
|
Serial.println();
|
||||||
|
Serial.print("TIME: \t");
|
||||||
|
Serial.println(stop - start);
|
||||||
|
Serial.println("done...");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// -- END OF FILE --
|
@ -0,0 +1,7 @@
|
|||||||
|
|
||||||
|
IDE: 1.8.19
|
||||||
|
Board: UNO:
|
||||||
|
version: 0.1.5
|
||||||
|
|
||||||
|
Average angle: 61.56 deg
|
||||||
|
Average time: 681.09 CPU cycles
|
@ -0,0 +1,10 @@
|
|||||||
|
|
||||||
|
IDE: 1.8.19
|
||||||
|
Board: UNO:
|
||||||
|
version: 0.2.0
|
||||||
|
|
||||||
|
both mode 0 and mode 1
|
||||||
|
|
||||||
|
Average angle: 61.56 deg
|
||||||
|
Average time: 681.09 CPU cycles
|
||||||
|
|
@ -1,15 +1,15 @@
|
|||||||
/*
|
/*
|
||||||
* time-add.ino: Measure the average execution time of
|
time-add.ino: Measure the average execution time of
|
||||||
* runningAngle::add().
|
runningAngle::add().
|
||||||
*
|
|
||||||
* This test sketch feeds pseudo-random angles to runningAngle::add()
|
This test sketch feeds pseudo-random angles to runningAngle::add()
|
||||||
* in order to measure its average execution time in CPU cycles. The
|
in order to measure its average execution time in CPU cycles. The
|
||||||
* input angles are within 0..90 deg, which ensures there will be no
|
input angles are within 0..90 deg, which ensures there will be no
|
||||||
* wrapping. Wrapping would make the method slightly slower, but it is
|
wrapping. Wrapping would make the method slightly slower, but it is
|
||||||
* expected to be infrequent in typical use cases.
|
expected to be infrequent in typical use cases.
|
||||||
*
|
|
||||||
* This test is meant to run on AVR-based Arduinos only.
|
This test is meant to run on AVR-based Arduinos only.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#include <runningAngle.h>
|
#include <runningAngle.h>
|
||||||
@ -47,61 +47,62 @@ const int iterations = 100;
|
|||||||
// computation of the input angle within the timed portion of the code.
|
// computation of the input angle within the timed portion of the code.
|
||||||
static float unoptimize(float x)
|
static float unoptimize(float x)
|
||||||
{
|
{
|
||||||
volatile float y = x;
|
volatile float y = x;
|
||||||
return y;
|
return y;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
runningAngle heading(runningAngle::ANGLE_UNIT);
|
runningAngle heading(runningAngle::ANGLE_UNIT);
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
Serial.begin(9600);
|
Serial.begin(9600);
|
||||||
|
|
||||||
// Set Timer 1 to count in normal mode at the full CPU frequency.
|
heading.setMode1();
|
||||||
// The timer value, TCNT1, can then be used as a clock with
|
|
||||||
// single-cycle resolution.
|
|
||||||
TCCR1A = 0;
|
|
||||||
TCCR1B = _BV(CS10);
|
|
||||||
|
|
||||||
long total_time = 0;
|
// Set Timer 1 to count in normal mode at the full CPU frequency.
|
||||||
for (int i = 0; i <= iterations; i++) {
|
// The timer value, TCNT1, can then be used as a clock with
|
||||||
float angle = (rand() + 0.5) / (RAND_MAX + 1.0) * ANGLE_MAX;
|
// single-cycle resolution.
|
||||||
angle = unoptimize(angle);
|
TCCR1A = 0;
|
||||||
|
TCCR1B = _BV(CS10);
|
||||||
|
|
||||||
// Timed part.
|
long total_time = 0;
|
||||||
uint16_t start_time = TCNT1;
|
for (int i = 0; i <= iterations; i++) {
|
||||||
heading.add(angle);
|
float angle = (rand() + 0.5) / (RAND_MAX + 1.0) * ANGLE_MAX;
|
||||||
uint16_t end_time = TCNT1;
|
angle = unoptimize(angle);
|
||||||
|
|
||||||
// Do not use the time of the first execution of add(), as it
|
// Timed part.
|
||||||
// goes through a non typical and much shorter execution path.
|
uint16_t start_time = TCNT1;
|
||||||
if (i != 0) {
|
heading.add(angle);
|
||||||
|
uint16_t end_time = TCNT1;
|
||||||
|
|
||||||
// Add the execution time of this iteration. Note that timer
|
// Do not use the time of the first execution of add(), as it
|
||||||
// rollover is not an issue as long as the timed code takes
|
// goes through a non typical and much shorter execution path.
|
||||||
// less that 65536 cycles, and the timing computations are
|
if (i != 0) {
|
||||||
// performed in uint16_t, as this type rolls over in the
|
|
||||||
// same manner as the timer itself.
|
// Add the execution time of this iteration. Note that timer
|
||||||
total_time += end_time - start_time - timing_overhead;
|
// rollover is not an issue as long as the timed code takes
|
||||||
}
|
// less that 65536 cycles, and the timing computations are
|
||||||
|
// performed in uint16_t, as this type rolls over in the
|
||||||
|
// same manner as the timer itself.
|
||||||
|
total_time += end_time - start_time - timing_overhead;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// Use the resulting average. Otherwise the whole averaging code
|
// Use the resulting average. Otherwise the whole averaging code
|
||||||
// could be optimized away.
|
// could be optimized away.
|
||||||
Serial.print("Average angle: ");
|
Serial.print("Average angle: ");
|
||||||
Serial.print(heading.getAverage());
|
Serial.print(heading.getAverage());
|
||||||
Serial.println(" " ANGLE_SYMBOL);
|
Serial.println(" " ANGLE_SYMBOL);
|
||||||
|
|
||||||
// Print the timing result.
|
// Print the timing result.
|
||||||
Serial.print("Average time: ");
|
Serial.print("Average time: ");
|
||||||
Serial.print((float) total_time / iterations);
|
Serial.print((float) total_time / iterations);
|
||||||
Serial.println(" CPU cycles");
|
Serial.println(" CPU cycles");
|
||||||
|
|
||||||
// This can be used to exit a simulation on simavr.
|
// This can be used to exit a simulation on simavr.
|
||||||
Serial.flush();
|
Serial.flush();
|
||||||
cli();
|
cli();
|
||||||
asm("sleep");
|
asm("sleep");
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop(){}
|
void loop() {}
|
||||||
|
|
||||||
|
@ -17,6 +17,11 @@ getWeight KEYWORD2
|
|||||||
type KEYWORD2
|
type KEYWORD2
|
||||||
wrap KEYWORD2
|
wrap KEYWORD2
|
||||||
|
|
||||||
|
setMode0 KEYWORD2
|
||||||
|
setMode1 KEYWORD2
|
||||||
|
getMode1 KEYWORD2
|
||||||
|
|
||||||
|
|
||||||
# Constants (LITERAL1)
|
# Constants (LITERAL1)
|
||||||
RUNNING_ANGLE_LIB_VERSION LITERAL1
|
RUNNING_ANGLE_LIB_VERSION LITERAL1
|
||||||
|
|
||||||
@ -24,3 +29,7 @@ DEGREES LITERAL1
|
|||||||
RADIANS LITERAL1
|
RADIANS LITERAL1
|
||||||
GRADIANS LITERAL1
|
GRADIANS LITERAL1
|
||||||
|
|
||||||
|
RA_DEFAULT_WEIGHT LITERAL1
|
||||||
|
RA_MIN_WEIGHT LITERAL1
|
||||||
|
RA_MAX_WEIGHT LITERAL1
|
||||||
|
|
||||||
|
@ -15,7 +15,7 @@
|
|||||||
"type": "git",
|
"type": "git",
|
||||||
"url": "https://github.com/RobTillaart/runningAngle.git"
|
"url": "https://github.com/RobTillaart/runningAngle.git"
|
||||||
},
|
},
|
||||||
"version": "0.1.5",
|
"version": "0.2.0",
|
||||||
"license": "MIT",
|
"license": "MIT",
|
||||||
"frameworks": "arduino",
|
"frameworks": "arduino",
|
||||||
"platforms": "*",
|
"platforms": "*",
|
||||||
|
@ -1,9 +1,9 @@
|
|||||||
name=runningAngle
|
name=runningAngle
|
||||||
version=0.1.5
|
version=0.2.0
|
||||||
author=Rob Tillaart <rob.tillaart@gmail.com>
|
author=Rob Tillaart <rob.tillaart@gmail.com>
|
||||||
maintainer=Rob Tillaart <rob.tillaart@gmail.com>
|
maintainer=Rob Tillaart <rob.tillaart@gmail.com>
|
||||||
sentence=Library to average angles by means of low pass filtering with wrapping.
|
sentence=Library to average angles by means of low pass filtering with wrapping.
|
||||||
paragraph=
|
paragraph=
|
||||||
category=Data Processing
|
category=Data Processing
|
||||||
url=https://github.com/RobTillaart/runningAngle
|
url=https://github.com/RobTillaart/runningAngle
|
||||||
architectures=*
|
architectures=*
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
//
|
//
|
||||||
// FILE: runningAngle.cpp
|
// FILE: runningAngle.cpp
|
||||||
// AUTHOR: Rob Tillaart
|
// AUTHOR: Rob Tillaart
|
||||||
// VERSION: 0.1.5
|
// VERSION: 0.2.0
|
||||||
// PURPOSE: Library to average angles by means of low pass filtering with wrapping.
|
// PURPOSE: Library to average angles by means of low pass filtering with wrapping.
|
||||||
// URL: https://github.com/RobTillaart/runningAngle
|
// URL: https://github.com/RobTillaart/runningAngle
|
||||||
// RELATED: https://github.com/RobTillaart/AverageAngle
|
// RELATED: https://github.com/RobTillaart/AverageAngle
|
||||||
@ -19,9 +19,9 @@ runningAngle::runningAngle(const enum AngleType type)
|
|||||||
|
|
||||||
void runningAngle::reset()
|
void runningAngle::reset()
|
||||||
{
|
{
|
||||||
_average = 0;
|
_average = 0;
|
||||||
_weight = 0.80;
|
_weight = RA_DEFAULT_WEIGHT;
|
||||||
_reset = true;
|
_reset = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -29,14 +29,56 @@ float runningAngle::add(float angle)
|
|||||||
{
|
{
|
||||||
if (_reset)
|
if (_reset)
|
||||||
{
|
{
|
||||||
_average = angle;
|
_average = wrap(angle);
|
||||||
_reset = false;
|
_reset = false;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
_average = wrap(_average + _weight * wrap(angle - _average));
|
_average = wrap(_average + _weight * wrap(angle - _average));
|
||||||
}
|
}
|
||||||
return _average;
|
if (_mode == 0) return _average;
|
||||||
|
return getAverage();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
float runningAngle::getAverage()
|
||||||
|
{
|
||||||
|
if (_mode == 0) return _average;
|
||||||
|
if (_average >= 0) return _average;
|
||||||
|
if (_type == DEGREES) return _average + 360;
|
||||||
|
if (_type == RADIANS) return _average + TWO_PI;
|
||||||
|
// GRADIANS
|
||||||
|
return _average + 200;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool runningAngle::setWeight(float w)
|
||||||
|
{
|
||||||
|
if (w < RA_MIN_WEIGHT)
|
||||||
|
{
|
||||||
|
_weight = RA_MIN_WEIGHT;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (w > RA_MAX_WEIGHT)
|
||||||
|
{
|
||||||
|
_weight = RA_MAX_WEIGHT;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
_weight = w;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
float runningAngle::getWeight()
|
||||||
|
{
|
||||||
|
return _weight;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
enum runningAngle::AngleType runningAngle::type()
|
||||||
|
{
|
||||||
|
return _type;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -44,22 +86,42 @@ float runningAngle::wrap(float angle)
|
|||||||
{
|
{
|
||||||
if (_type == DEGREES)
|
if (_type == DEGREES)
|
||||||
{
|
{
|
||||||
while (angle < -180) angle += 360;
|
while (angle < -180) angle += 360;
|
||||||
while (angle >= 180) angle -= 360;
|
while (angle >= +180) angle -= 360;
|
||||||
}
|
}
|
||||||
else if (_type == RADIANS)
|
else if (_type == RADIANS)
|
||||||
{
|
{
|
||||||
while (angle < -PI) angle += TWO_PI;
|
while (angle < -PI) angle += TWO_PI;
|
||||||
while (angle >= PI) angle -= TWO_PI;
|
while (angle >= +PI) angle -= TWO_PI;
|
||||||
}
|
}
|
||||||
else // GRADIANS
|
else // GRADIANS
|
||||||
{
|
{
|
||||||
while (angle < -200) angle += 400;
|
while (angle < -200) angle += 400;
|
||||||
while (angle >= 200) angle -= 400;
|
while (angle >= +200) angle -= 400;
|
||||||
}
|
}
|
||||||
return angle;
|
return angle;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// -180..180
|
||||||
|
void runningAngle::setMode0()
|
||||||
|
{
|
||||||
|
_mode = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// 0..360
|
||||||
|
void runningAngle::setMode1()
|
||||||
|
{
|
||||||
|
_mode = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t runningAngle::getMode()
|
||||||
|
{
|
||||||
|
return _mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// -- END OF FILE --
|
// -- END OF FILE --
|
||||||
|
|
||||||
|
@ -2,7 +2,7 @@
|
|||||||
//
|
//
|
||||||
// FILE: runningAngle.h
|
// FILE: runningAngle.h
|
||||||
// AUTHOR: Rob Tillaart
|
// AUTHOR: Rob Tillaart
|
||||||
// VERSION: 0.1.5
|
// VERSION: 0.2.0
|
||||||
// PURPOSE: Library to average angles by means of low pass filtering with wrapping.
|
// PURPOSE: Library to average angles by means of low pass filtering with wrapping.
|
||||||
// URL: https://github.com/RobTillaart/runningAngle
|
// URL: https://github.com/RobTillaart/runningAngle
|
||||||
// RELATED: https://github.com/RobTillaart/AverageAngle
|
// RELATED: https://github.com/RobTillaart/AverageAngle
|
||||||
@ -12,7 +12,12 @@
|
|||||||
#include "math.h"
|
#include "math.h"
|
||||||
|
|
||||||
|
|
||||||
#define RUNNING_ANGLE_LIB_VERSION (F("0.1.5"))
|
#define RUNNING_ANGLE_LIB_VERSION (F("0.2.0"))
|
||||||
|
|
||||||
|
|
||||||
|
const float RA_DEFAULT_WEIGHT = 0.80;
|
||||||
|
const float RA_MIN_WEIGHT = 0.001;
|
||||||
|
const float RA_MAX_WEIGHT = 1.0;
|
||||||
|
|
||||||
|
|
||||||
class runningAngle
|
class runningAngle
|
||||||
@ -20,26 +25,31 @@ class runningAngle
|
|||||||
public:
|
public:
|
||||||
enum AngleType { DEGREES = 0, RADIANS = 1, GRADIANS = 2 };
|
enum AngleType { DEGREES = 0, RADIANS = 1, GRADIANS = 2 };
|
||||||
|
|
||||||
|
|
||||||
runningAngle(const enum AngleType type = DEGREES);
|
runningAngle(const enum AngleType type = DEGREES);
|
||||||
|
|
||||||
// first value added will not use the weight to set the initial value.
|
// first value added will not use the weight to set the initial value.
|
||||||
float add(float angle); // returns new average
|
float add(float angle); // returns new average
|
||||||
void reset();
|
void reset();
|
||||||
float getAverage() { return _average; };
|
float getAverage();
|
||||||
|
|
||||||
void setWeight(float w) { _weight = constrain(w, 0.001, 1); };
|
bool setWeight(float w = RA_DEFAULT_WEIGHT);
|
||||||
float getWeight() { return _weight; };
|
float getWeight();
|
||||||
enum AngleType type() { return _type; };
|
enum AngleType type();
|
||||||
|
|
||||||
// reformat angle to -180..+180 (degrees) or -PI..PI (radians)
|
// reformat angle to -180..+180 (degrees) or -PI..PI (radians)
|
||||||
float wrap(float angle);
|
float wrap(float angle);
|
||||||
|
|
||||||
|
// select the output
|
||||||
|
void setMode0(); // -180..180
|
||||||
|
void setMode1(); // 0..360
|
||||||
|
uint8_t getMode();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
enum AngleType _type;
|
enum AngleType _type;
|
||||||
float _average = 0;
|
float _average = 0;
|
||||||
float _weight;
|
float _weight;
|
||||||
bool _reset;
|
bool _reset;
|
||||||
|
uint16_t _mode = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -45,27 +45,35 @@ unittest_teardown()
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
unittest(test_constants)
|
||||||
|
{
|
||||||
|
assertEqualFloat(0.800, RA_DEFAULT_WEIGHT, 0.0001);
|
||||||
|
assertEqualFloat(0.001, RA_MIN_WEIGHT, 0.0001);
|
||||||
|
assertEqualFloat(1.000, RA_MAX_WEIGHT, 0.0001);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
unittest(test_constructor_1)
|
unittest(test_constructor_1)
|
||||||
{
|
{
|
||||||
runningAngle heading(runningAngle::DEGREES);
|
runningAngle heading(runningAngle::DEGREES);
|
||||||
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
||||||
assertEqualFloat(0, heading.getAverage(), 0.0001);
|
assertEqualFloat(0.00, heading.getAverage(), 0.0001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
unittest(test_constructor_2)
|
unittest(test_constructor_2)
|
||||||
{
|
{
|
||||||
runningAngle heading(runningAngle::RADIANS);
|
runningAngle heading(runningAngle::RADIANS);
|
||||||
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
||||||
assertEqualFloat(0, heading.getAverage(), 0.0001);
|
assertEqualFloat(0.00, heading.getAverage(), 0.0001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
unittest(test_constructor_3)
|
unittest(test_constructor_3)
|
||||||
{
|
{
|
||||||
runningAngle heading(runningAngle::GRADIANS);
|
runningAngle heading(runningAngle::GRADIANS);
|
||||||
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
||||||
assertEqualFloat(0, heading.getAverage(), 0.0001);
|
assertEqualFloat(0.00, heading.getAverage(), 0.0001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -90,15 +98,18 @@ unittest(test_weight)
|
|||||||
runningAngle heading(runningAngle::DEGREES);
|
runningAngle heading(runningAngle::DEGREES);
|
||||||
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
||||||
|
|
||||||
heading.setWeight(0.85);
|
assertTrue(heading.setWeight(0.85));
|
||||||
assertEqualFloat(0.85, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.85, heading.getWeight(), 0.0001);
|
||||||
|
|
||||||
heading.setWeight(2);
|
assertFalse(heading.setWeight(2));
|
||||||
assertEqualFloat(1, heading.getWeight(), 0.0001);
|
assertEqualFloat(1, heading.getWeight(), 0.0001);
|
||||||
|
|
||||||
heading.setWeight(-5);
|
assertFalse(heading.setWeight(-5));
|
||||||
assertEqualFloat(0.001, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.001, heading.getWeight(), 0.0001);
|
||||||
|
|
||||||
|
assertTrue(heading.setWeight()); // use default
|
||||||
|
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
||||||
|
|
||||||
fprintf(stderr, "\treset()\n");
|
fprintf(stderr, "\treset()\n");
|
||||||
heading.reset();
|
heading.reset();
|
||||||
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
assertEqualFloat(0.80, heading.getWeight(), 0.0001);
|
||||||
@ -109,6 +120,8 @@ unittest(test_wrap)
|
|||||||
{
|
{
|
||||||
runningAngle heading(runningAngle::DEGREES);
|
runningAngle heading(runningAngle::DEGREES);
|
||||||
|
|
||||||
|
heading.setMode0();
|
||||||
|
|
||||||
assertEqualFloat(0, heading.wrap(0), 0.0001);
|
assertEqualFloat(0, heading.wrap(0), 0.0001);
|
||||||
assertEqualFloat(0, heading.wrap(360), 0.0001);
|
assertEqualFloat(0, heading.wrap(360), 0.0001);
|
||||||
assertEqualFloat(1, heading.wrap(361), 0.0001);
|
assertEqualFloat(1, heading.wrap(361), 0.0001);
|
||||||
@ -125,6 +138,35 @@ unittest(test_wrap)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
unittest(test_mode_0)
|
||||||
|
{
|
||||||
|
runningAngle heading(runningAngle::DEGREES);
|
||||||
|
|
||||||
|
for (int i = 0; i < 360; i+= 20)
|
||||||
|
{
|
||||||
|
heading.reset();
|
||||||
|
heading.setMode0();
|
||||||
|
if (i < 180) assertEqualFloat(i, heading.add(i), 0.0001);
|
||||||
|
else assertEqualFloat(i-360, heading.add(i), 0.0001);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
unittest(test_mode_1)
|
||||||
|
{
|
||||||
|
runningAngle heading(runningAngle::DEGREES);
|
||||||
|
|
||||||
|
for (int i = 0; i < 360; i+= 20)
|
||||||
|
{
|
||||||
|
heading.reset();
|
||||||
|
heading.setMode1();
|
||||||
|
assertEqualFloat(i, heading.add(i), 0.0001);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
unittest_main()
|
unittest_main()
|
||||||
|
|
||||||
// --------
|
|
||||||
|
// -- END OF FILE --
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user