/*
update distance_cm
*/
void AP_RangeFinder_analog::update(void)
{
update_voltage();
float v = state.voltage_mv * 0.001f;
float dist_m = 0;
float scaling = params.scaling;
float offset = params.offset;
RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)params.function.get();
int16_t _max_distance_cm = params.max_distance_cm;
switch (function) {
case RangeFinder::FUNCTION_LINEAR:
dist_m = (v - offset) * scaling;
break;
case RangeFinder::FUNCTION_INVERTED:
dist_m = (offset - v) * scaling;
break;
case RangeFinder::FUNCTION_HYPERBOLA:
if (v <= offset) {
dist_m = 0;
} else {
dist_m = scaling / (v - offset);
}
if (dist_m > _max_distance_cm * 0.01f) {
dist_m = _max_distance_cm * 0.01f;
}
break;
}
if (dist_m < 0) {
dist_m = 0;
}
state.distance_cm = dist_m * 100.0f;
state.last_reading_ms = AP_HAL::millis();
// update range_valid state based on distance measured
update_status();
}
/*
update distance_cm
*/
void AP_RangeFinder_analog::update(void)
{
update_voltage();
float v = state.voltage_mv * 0.001f;
float dist_m = 0;
float scaling = ranger._scaling[state.instance];
float offset = ranger._offset[state.instance];
RangeFinder::RangeFinder_Function function = (RangeFinder::RangeFinder_Function)ranger._function[state.instance].get();
int16_t max_distance_cm = ranger._max_distance_cm[state.instance];
switch (function) {
case RangeFinder::FUNCTION_LINEAR:
dist_m = (v - offset) * scaling;
break;
case RangeFinder::FUNCTION_INVERTED:
dist_m = (offset - v) * scaling;
break;
case RangeFinder::FUNCTION_HYPERBOLA:
if (v <= offset) {
dist_m = 0;
}
dist_m = scaling / (v - offset);
if (isinf(dist_m) || dist_m > max_distance_cm * 0.01f) {
dist_m = max_distance_cm * 0.01f;
}
break;
}
if (dist_m < 0) {
dist_m = 0;
}
state.distance_cm = dist_m * 100.0f;
// update range_valid state based on distance measured
update_status();
}
