mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_RangeFinder: use enum-class for RangeFinder function
This commit is contained in:
parent
db36ef3433
commit
1989decbc1
@ -86,19 +86,19 @@ void AP_RangeFinder_analog::update(void)
|
|||||||
float dist_m = 0;
|
float dist_m = 0;
|
||||||
float scaling = params.scaling;
|
float scaling = params.scaling;
|
||||||
float offset = params.offset;
|
float offset = params.offset;
|
||||||
RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)params.function.get();
|
RangeFinder::Function function = (RangeFinder::Function)params.function.get();
|
||||||
int16_t _max_distance_cm = params.max_distance_cm;
|
int16_t _max_distance_cm = params.max_distance_cm;
|
||||||
|
|
||||||
switch (function) {
|
switch (function) {
|
||||||
case RangeFinder::FUNCTION_LINEAR:
|
case RangeFinder::Function::LINEAR:
|
||||||
dist_m = (v - offset) * scaling;
|
dist_m = (v - offset) * scaling;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case RangeFinder::FUNCTION_INVERTED:
|
case RangeFinder::Function::INVERTED:
|
||||||
dist_m = (offset - v) * scaling;
|
dist_m = (offset - v) * scaling;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case RangeFinder::FUNCTION_HYPERBOLA:
|
case RangeFinder::Function::HYPERBOLA:
|
||||||
if (v <= offset) {
|
if (v <= offset) {
|
||||||
dist_m = 0;
|
dist_m = 0;
|
||||||
} else {
|
} else {
|
||||||
|
@ -79,10 +79,10 @@ public:
|
|||||||
BenewakeTF03 = 27,
|
BenewakeTF03 = 27,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum RangeFinder_Function {
|
enum class Function {
|
||||||
FUNCTION_LINEAR = 0,
|
LINEAR = 0,
|
||||||
FUNCTION_INVERTED = 1,
|
INVERTED = 1,
|
||||||
FUNCTION_HYPERBOLA = 2
|
HYPERBOLA = 2
|
||||||
};
|
};
|
||||||
|
|
||||||
enum RangeFinder_Status {
|
enum RangeFinder_Status {
|
||||||
|
Loading…
Reference in New Issue
Block a user