AP_RangeFinder: use enum-class for Status

This commit is contained in:
Peter Barker 2019-11-01 16:10:52 +11:00 committed by Peter Barker
parent 1989decbc1
commit 82db4383d4
26 changed files with 55 additions and 55 deletions

View File

@ -110,7 +110,7 @@ bool AP_RangeFinder_BBB_PRU::detect()
*/ */
void AP_RangeFinder_BBB_PRU::update(void) void AP_RangeFinder_BBB_PRU::update(void)
{ {
state.status = (RangeFinder::RangeFinder_Status)rangerpru->status; state.status = (RangeFinder::Status)rangerpru->status;
state.distance_cm = rangerpru->distance; state.distance_cm = rangerpru->distance;
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
} }

View File

@ -85,7 +85,7 @@ void AP_RangeFinder_BLPing::update(void)
state.last_reading_ms = now; state.last_reading_ms = now;
update_status(); update_status();
} else if (now - state.last_reading_ms > BLPING_TIMEOUT_MS) { } else if (now - state.last_reading_ms > BLPING_TIMEOUT_MS) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
// initialise sensor if no distances recently // initialise sensor if no distances recently
if (now - last_init_ms > BLPING_INIT_RATE_MS) { if (now - last_init_ms > BLPING_INIT_RATE_MS) {

View File

@ -186,6 +186,6 @@ void AP_RangeFinder_Benewake::update(void)
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -145,7 +145,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update()
accum.count = 0; accum.count = 0;
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -120,6 +120,6 @@ void AP_RangeFinder_Lanbao::update(void)
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -133,7 +133,7 @@ void AP_RangeFinder_LeddarOne::update(void)
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -453,7 +453,7 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void)
// update range_valid state based on distance measured // update range_valid state based on distance measured
update_status(); update_status();
} else { } else {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }
@ -463,7 +463,7 @@ void AP_RangeFinder_LightWareI2C::sf20_timer(void)
// update range_valid state based on distance measured // update range_valid state based on distance measured
update_status(); update_status();
} else { } else {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -123,6 +123,6 @@ void AP_RangeFinder_LightWareSerial::update(void)
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -67,7 +67,7 @@ void AP_RangeFinder_MAVLink::update(void)
//Time out on incoming data; if we don't get new //Time out on incoming data; if we don't get new
//data in 500ms, dump it //data in 500ms, dump it
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) { if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
state.distance_cm = 0; state.distance_cm = 0;
} else { } else {
state.distance_cm = distance_cm; state.distance_cm = distance_cm;

View File

@ -155,6 +155,6 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void)
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 300) { } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
// if no updates for 0.3 seconds set no-data // if no updates for 0.3 seconds set no-data
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -98,6 +98,6 @@ void AP_RangeFinder_MaxsonarSerialLV::update(void)
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 500) { } else if (AP_HAL::millis() - state.last_reading_ms > 500) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -52,7 +52,7 @@ void AP_RangeFinder_NMEA::update(void)
state.last_reading_ms = now; state.last_reading_ms = now;
update_status(); update_status();
} else if ((now - state.last_reading_ms) > 3000) { } else if ((now - state.last_reading_ms) > 3000) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -151,7 +151,7 @@ void AP_RangeFinder_PWM::update(void)
if (!was_out_of_range) { if (!was_out_of_range) {
// we are above the power saving range. Disable the sensor // we are above the power saving range. Disable the sensor
hal.gpio->write(params.stop_pin, false); hal.gpio->write(params.stop_pin, false);
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
state.distance_cm = 0; state.distance_cm = 0;
state.voltage_mv = 0; state.voltage_mv = 0;
was_out_of_range = oor; was_out_of_range = oor;
@ -168,7 +168,7 @@ void AP_RangeFinder_PWM::update(void)
if (!get_reading(state.distance_cm)) { if (!get_reading(state.distance_cm)) {
// failure; consider changing our state // failure; consider changing our state
if (AP_HAL::millis() - state.last_reading_ms > 200) { if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
return; return;
} }

View File

@ -102,7 +102,7 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
} }
last_distance_cm = _distance_cm; last_distance_cm = _distance_cm;
} else { } else {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
if (!v2_hardware) { if (!v2_hardware) {
// for v2 hw we use continuous mode // for v2 hw we use continuous mode

View File

@ -191,6 +191,6 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
accum.count = 0; accum.count = 0;
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -102,14 +102,14 @@ void AP_RangeFinder_UAVCAN::update()
WITH_SEMAPHORE(_sem); WITH_SEMAPHORE(_sem);
if ((AP_HAL::millis() - _last_reading_ms) > 500) { if ((AP_HAL::millis() - _last_reading_ms) > 500) {
//if data is older than 500ms, report NoData //if data is older than 500ms, report NoData
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} else if (_status == RangeFinder::RangeFinder_Good && new_data) { } else if (_status == RangeFinder::Status::Good && new_data) {
//copy over states //copy over states
state.distance_cm = _distance_cm; state.distance_cm = _distance_cm;
state.last_reading_ms = _last_reading_ms; state.last_reading_ms = _last_reading_ms;
update_status(); update_status();
new_data = false; new_data = false;
} else if (_status != RangeFinder::RangeFinder_Good) { } else if (_status != RangeFinder::Status::Good) {
//handle additional states received by measurement handler //handle additional states received by measurement handler
set_status(_status); set_status(_status);
} }
@ -130,7 +130,7 @@ void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod
//update the states in backend instance //update the states in backend instance
driver->_distance_cm = cb.msg->range*100.0f; driver->_distance_cm = cb.msg->range*100.0f;
driver->_last_reading_ms = AP_HAL::millis(); driver->_last_reading_ms = AP_HAL::millis();
driver->_status = RangeFinder::RangeFinder_Good; driver->_status = RangeFinder::Status::Good;
driver->new_data = true; driver->new_data = true;
break; break;
} }
@ -138,13 +138,13 @@ void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod
case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE: case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE:
{ {
driver->_last_reading_ms = AP_HAL::millis(); driver->_last_reading_ms = AP_HAL::millis();
driver->_status = RangeFinder::RangeFinder_OutOfRangeLow; driver->_status = RangeFinder::Status::OutOfRangeLow;
break; break;
} }
case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR: case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR:
{ {
driver->_last_reading_ms = AP_HAL::millis(); driver->_last_reading_ms = AP_HAL::millis();
driver->_status = RangeFinder::RangeFinder_OutOfRangeHigh; driver->_status = RangeFinder::Status::OutOfRangeHigh;
break; break;
} }
default: default:

View File

@ -25,7 +25,7 @@ protected:
} }
private: private:
uint8_t _instance; uint8_t _instance;
RangeFinder::RangeFinder_Status _status; RangeFinder::Status _status;
uint16_t _distance_cm; uint16_t _distance_cm;
uint32_t _last_reading_ms; uint32_t _last_reading_ms;
AP_UAVCAN* _ap_uavcan; AP_UAVCAN* _ap_uavcan;

View File

@ -773,7 +773,7 @@ void AP_RangeFinder_VL53L0X::update(void)
counter = 0; counter = 0;
update_status(); update_status();
} else { } else {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -555,6 +555,6 @@ void AP_RangeFinder_VL53L1X::update(void)
counter = 0; counter = 0;
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
// if no updates for 0.2s set no-data // if no updates for 0.2s set no-data
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -133,7 +133,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
} }
reading_cm = 100 * sum / count; reading_cm = 100 * sum / count;
set_status(RangeFinder::RangeFinder_Good); set_status(RangeFinder::Status::Good);
return true; return true;
} }
@ -142,7 +142,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
void AP_RangeFinder_Wasp::update(void) { void AP_RangeFinder_Wasp::update(void) {
if (!get_reading(state.distance_cm)) { if (!get_reading(state.distance_cm)) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
if (AP_HAL::millis() - state.last_reading_ms > 500) { if (AP_HAL::millis() - state.last_reading_ms > 500) {

View File

@ -38,10 +38,10 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_st
source = hal.analogin->channel(_params.pin); source = hal.analogin->channel(_params.pin);
if (source == nullptr) { if (source == nullptr) {
// failed to allocate a ADC channel? This shouldn't happen // failed to allocate a ADC channel? This shouldn't happen
set_status(RangeFinder::RangeFinder_NotConnected); set_status(RangeFinder::Status::NotConnected);
return; return;
} }
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
/* /*

View File

@ -215,6 +215,6 @@ void AP_RangeFinder_uLanding::update(void)
// update range_valid state based on distance measured // update range_valid state based on distance measured
update_status(); update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) { } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::Status::NoData);
} }
} }

View File

@ -316,7 +316,7 @@ void RangeFinder::init(enum Rotation orientation_default)
} }
// initialise status // initialise status
state[i].status = RangeFinder_NotConnected; state[i].status = Status::NotConnected;
state[i].range_valid_count = 0; state[i].range_valid_count = 0;
} }
} }
@ -331,7 +331,7 @@ void RangeFinder::update(void)
if (drivers[i] != nullptr) { if (drivers[i] != nullptr) {
if ((Type)params[i].type.get() == Type::NONE) { if ((Type)params[i].type.get() == Type::NONE) {
// allow user to disable a rangefinder at runtime // allow user to disable a rangefinder at runtime
state[i].status = RangeFinder_NotConnected; state[i].status = Status::NotConnected;
state[i].range_valid_count = 0; state[i].range_valid_count = 0;
continue; continue;
} }
@ -556,11 +556,11 @@ AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const {
return drivers[id]; return drivers[id];
}; };
RangeFinder::RangeFinder_Status RangeFinder::status_orient(enum Rotation orientation) const RangeFinder::Status RangeFinder::status_orient(enum Rotation orientation) const
{ {
AP_RangeFinder_Backend *backend = find_instance(orientation); AP_RangeFinder_Backend *backend = find_instance(orientation);
if (backend == nullptr) { if (backend == nullptr) {
return RangeFinder_NotConnected; return Status::NotConnected;
} }
return backend->status(); return backend->status();
} }
@ -589,7 +589,7 @@ AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) co
AP_RangeFinder_Backend *backend = get_backend(i); AP_RangeFinder_Backend *backend = get_backend(i);
if (backend != nullptr && if (backend != nullptr &&
backend->orientation() == orientation && backend->orientation() == orientation &&
backend->status() == RangeFinder_Good) { backend->status() == Status::Good) {
return backend; return backend;
} }
} }

View File

@ -85,19 +85,19 @@ public:
HYPERBOLA = 2 HYPERBOLA = 2
}; };
enum RangeFinder_Status { enum class Status {
RangeFinder_NotConnected = 0, NotConnected = 0,
RangeFinder_NoData, NoData,
RangeFinder_OutOfRangeLow, OutOfRangeLow,
RangeFinder_OutOfRangeHigh, OutOfRangeHigh,
RangeFinder_Good Good
}; };
// The RangeFinder_State structure is filled in by the backend driver // The RangeFinder_State structure is filled in by the backend driver
struct RangeFinder_State { struct RangeFinder_State {
uint16_t distance_cm; // distance: in cm uint16_t distance_cm; // distance: in cm
uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0 uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
enum RangeFinder_Status status; // sensor status enum RangeFinder::Status status; // sensor status
uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10) uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10)
uint32_t last_reading_ms; // system time of last successful update from sensor uint32_t last_reading_ms; // system time of last successful update from sensor
@ -145,7 +145,7 @@ public:
int16_t min_distance_cm_orient(enum Rotation orientation) const; int16_t min_distance_cm_orient(enum Rotation orientation) const;
int16_t ground_clearance_cm_orient(enum Rotation orientation) const; int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const; MAV_DISTANCE_SENSOR get_mav_distance_sensor_type_orient(enum Rotation orientation) const;
RangeFinder_Status status_orient(enum Rotation orientation) const; RangeFinder::Status status_orient(enum Rotation orientation) const;
bool has_data_orient(enum Rotation orientation) const; bool has_data_orient(enum Rotation orientation) const;
uint8_t range_valid_count_orient(enum Rotation orientation) const; uint8_t range_valid_count_orient(enum Rotation orientation) const;
const Vector3f &get_pos_offset_orient(enum Rotation orientation) const; const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;

View File

@ -38,18 +38,18 @@ MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const
return _get_mav_distance_sensor_type(); return _get_mav_distance_sensor_type();
} }
RangeFinder::RangeFinder_Status AP_RangeFinder_Backend::status() const { RangeFinder::Status AP_RangeFinder_Backend::status() const {
if (type() == RangeFinder::Type::NONE) { if (type() == RangeFinder::Type::NONE) {
// turned off at runtime? // turned off at runtime?
return RangeFinder::RangeFinder_NotConnected; return RangeFinder::Status::NotConnected;
} }
return state.status; return state.status;
} }
// true if sensor is returning data // true if sensor is returning data
bool AP_RangeFinder_Backend::has_data() const { bool AP_RangeFinder_Backend::has_data() const {
return ((state.status != RangeFinder::RangeFinder_NotConnected) && return ((state.status != RangeFinder::Status::NotConnected) &&
(state.status != RangeFinder::RangeFinder_NoData)); (state.status != RangeFinder::Status::NoData));
} }
// update status based on distance measurement // update status based on distance measurement
@ -57,21 +57,21 @@ void AP_RangeFinder_Backend::update_status()
{ {
// check distance // check distance
if ((int16_t)state.distance_cm > params.max_distance_cm) { if ((int16_t)state.distance_cm > params.max_distance_cm) {
set_status(RangeFinder::RangeFinder_OutOfRangeHigh); set_status(RangeFinder::Status::OutOfRangeHigh);
} else if ((int16_t)state.distance_cm < params.min_distance_cm) { } else if ((int16_t)state.distance_cm < params.min_distance_cm) {
set_status(RangeFinder::RangeFinder_OutOfRangeLow); set_status(RangeFinder::Status::OutOfRangeLow);
} else { } else {
set_status(RangeFinder::RangeFinder_Good); set_status(RangeFinder::Status::Good);
} }
} }
// set status and update valid count // set status and update valid count
void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status _status) void AP_RangeFinder_Backend::set_status(RangeFinder::Status _status)
{ {
state.status = _status; state.status = _status;
// update valid count // update valid count
if (_status == RangeFinder::RangeFinder_Good) { if (_status == RangeFinder::Status::Good) {
if (state.range_valid_count < 10) { if (state.range_valid_count < 10) {
state.range_valid_count++; state.range_valid_count++;
} }

View File

@ -40,7 +40,7 @@ public:
int16_t min_distance_cm() const { return params.min_distance_cm; } int16_t min_distance_cm() const { return params.min_distance_cm; }
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; } int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const;
RangeFinder::RangeFinder_Status status() const; RangeFinder::Status status() const;
RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); } RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); }
// true if sensor is returning data // true if sensor is returning data
@ -62,7 +62,7 @@ protected:
void update_status(); void update_status();
// set status and update valid_count // set status and update valid_count
void set_status(RangeFinder::RangeFinder_Status status); void set_status(RangeFinder::Status status);
RangeFinder::RangeFinder_State &state; RangeFinder::RangeFinder_State &state;
AP_RangeFinder_Params &params; AP_RangeFinder_Params &params;