mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
AP_RangeFinder: use enum-class for Status
This commit is contained in:
parent
1989decbc1
commit
82db4383d4
libraries/AP_RangeFinder
AP_RangeFinder_BBB_PRU.cppAP_RangeFinder_BLPing.cppAP_RangeFinder_Benewake.cppAP_RangeFinder_Benewake_TFMiniPlus.cppAP_RangeFinder_Lanbao.cppAP_RangeFinder_LeddarOne.cppAP_RangeFinder_LightWareI2C.cppAP_RangeFinder_LightWareSerial.cppAP_RangeFinder_MAVLink.cppAP_RangeFinder_MaxsonarI2CXL.cppAP_RangeFinder_MaxsonarSerialLV.cppAP_RangeFinder_NMEA.cppAP_RangeFinder_PWM.cppAP_RangeFinder_PulsedLightLRF.cppAP_RangeFinder_TeraRangerI2C.cppAP_RangeFinder_UAVCAN.cppAP_RangeFinder_UAVCAN.hAP_RangeFinder_VL53L0X.cppAP_RangeFinder_VL53L1X.cppAP_RangeFinder_Wasp.cppAP_RangeFinder_analog.cppAP_RangeFinder_uLanding.cppRangeFinder.cppRangeFinder.hRangeFinder_Backend.cppRangeFinder_Backend.h
@ -110,7 +110,7 @@ bool AP_RangeFinder_BBB_PRU::detect()
|
||||
*/
|
||||
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.last_reading_ms = AP_HAL::millis();
|
||||
}
|
||||
|
@ -85,7 +85,7 @@ void AP_RangeFinder_BLPing::update(void)
|
||||
state.last_reading_ms = now;
|
||||
update_status();
|
||||
} 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
|
||||
if (now - last_init_ms > BLPING_INIT_RATE_MS) {
|
||||
|
@ -186,6 +186,6 @@ void AP_RangeFinder_Benewake::update(void)
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -145,7 +145,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update()
|
||||
accum.count = 0;
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -120,6 +120,6 @@ void AP_RangeFinder_Lanbao::update(void)
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -133,7 +133,7 @@ void AP_RangeFinder_LeddarOne::update(void)
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -453,7 +453,7 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void)
|
||||
// update range_valid state based on distance measured
|
||||
update_status();
|
||||
} 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_status();
|
||||
} else {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -123,6 +123,6 @@ void AP_RangeFinder_LightWareSerial::update(void)
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -67,7 +67,7 @@ void AP_RangeFinder_MAVLink::update(void)
|
||||
//Time out on incoming data; if we don't get new
|
||||
//data in 500ms, dump it
|
||||
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;
|
||||
} else {
|
||||
state.distance_cm = distance_cm;
|
||||
|
@ -155,6 +155,6 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void)
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 300) {
|
||||
// if no updates for 0.3 seconds set no-data
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -98,6 +98,6 @@ void AP_RangeFinder_MaxsonarSerialLV::update(void)
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 500) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -52,7 +52,7 @@ void AP_RangeFinder_NMEA::update(void)
|
||||
state.last_reading_ms = now;
|
||||
update_status();
|
||||
} else if ((now - state.last_reading_ms) > 3000) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -151,7 +151,7 @@ void AP_RangeFinder_PWM::update(void)
|
||||
if (!was_out_of_range) {
|
||||
// we are above the power saving range. Disable the sensor
|
||||
hal.gpio->write(params.stop_pin, false);
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
state.distance_cm = 0;
|
||||
state.voltage_mv = 0;
|
||||
was_out_of_range = oor;
|
||||
@ -168,7 +168,7 @@ void AP_RangeFinder_PWM::update(void)
|
||||
if (!get_reading(state.distance_cm)) {
|
||||
// failure; consider changing our state
|
||||
if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
@ -102,7 +102,7 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
|
||||
}
|
||||
last_distance_cm = _distance_cm;
|
||||
} else {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
if (!v2_hardware) {
|
||||
// for v2 hw we use continuous mode
|
||||
|
@ -191,6 +191,6 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
|
||||
accum.count = 0;
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -102,14 +102,14 @@ void AP_RangeFinder_UAVCAN::update()
|
||||
WITH_SEMAPHORE(_sem);
|
||||
if ((AP_HAL::millis() - _last_reading_ms) > 500) {
|
||||
//if data is older than 500ms, report NoData
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
} else if (_status == RangeFinder::RangeFinder_Good && new_data) {
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
} else if (_status == RangeFinder::Status::Good && new_data) {
|
||||
//copy over states
|
||||
state.distance_cm = _distance_cm;
|
||||
state.last_reading_ms = _last_reading_ms;
|
||||
update_status();
|
||||
new_data = false;
|
||||
} else if (_status != RangeFinder::RangeFinder_Good) {
|
||||
} else if (_status != RangeFinder::Status::Good) {
|
||||
//handle additional states received by measurement handler
|
||||
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
|
||||
driver->_distance_cm = cb.msg->range*100.0f;
|
||||
driver->_last_reading_ms = AP_HAL::millis();
|
||||
driver->_status = RangeFinder::RangeFinder_Good;
|
||||
driver->_status = RangeFinder::Status::Good;
|
||||
driver->new_data = true;
|
||||
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:
|
||||
{
|
||||
driver->_last_reading_ms = AP_HAL::millis();
|
||||
driver->_status = RangeFinder::RangeFinder_OutOfRangeLow;
|
||||
driver->_status = RangeFinder::Status::OutOfRangeLow;
|
||||
break;
|
||||
}
|
||||
case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR:
|
||||
{
|
||||
driver->_last_reading_ms = AP_HAL::millis();
|
||||
driver->_status = RangeFinder::RangeFinder_OutOfRangeHigh;
|
||||
driver->_status = RangeFinder::Status::OutOfRangeHigh;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
|
@ -25,7 +25,7 @@ protected:
|
||||
}
|
||||
private:
|
||||
uint8_t _instance;
|
||||
RangeFinder::RangeFinder_Status _status;
|
||||
RangeFinder::Status _status;
|
||||
uint16_t _distance_cm;
|
||||
uint32_t _last_reading_ms;
|
||||
AP_UAVCAN* _ap_uavcan;
|
||||
|
@ -773,7 +773,7 @@ void AP_RangeFinder_VL53L0X::update(void)
|
||||
counter = 0;
|
||||
update_status();
|
||||
} else {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -555,6 +555,6 @@ void AP_RangeFinder_VL53L1X::update(void)
|
||||
counter = 0;
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
// if no updates for 0.2s set no-data
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -133,7 +133,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
|
||||
}
|
||||
|
||||
reading_cm = 100 * sum / count;
|
||||
set_status(RangeFinder::RangeFinder_Good);
|
||||
set_status(RangeFinder::Status::Good);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -142,7 +142,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
|
||||
|
||||
void AP_RangeFinder_Wasp::update(void) {
|
||||
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) {
|
||||
|
@ -38,10 +38,10 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_st
|
||||
source = hal.analogin->channel(_params.pin);
|
||||
if (source == nullptr) {
|
||||
// failed to allocate a ADC channel? This shouldn't happen
|
||||
set_status(RangeFinder::RangeFinder_NotConnected);
|
||||
set_status(RangeFinder::Status::NotConnected);
|
||||
return;
|
||||
}
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -215,6 +215,6 @@ void AP_RangeFinder_uLanding::update(void)
|
||||
// update range_valid state based on distance measured
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
set_status(RangeFinder::Status::NoData);
|
||||
}
|
||||
}
|
||||
|
@ -316,7 +316,7 @@ void RangeFinder::init(enum Rotation orientation_default)
|
||||
}
|
||||
|
||||
// initialise status
|
||||
state[i].status = RangeFinder_NotConnected;
|
||||
state[i].status = Status::NotConnected;
|
||||
state[i].range_valid_count = 0;
|
||||
}
|
||||
}
|
||||
@ -331,7 +331,7 @@ void RangeFinder::update(void)
|
||||
if (drivers[i] != nullptr) {
|
||||
if ((Type)params[i].type.get() == Type::NONE) {
|
||||
// allow user to disable a rangefinder at runtime
|
||||
state[i].status = RangeFinder_NotConnected;
|
||||
state[i].status = Status::NotConnected;
|
||||
state[i].range_valid_count = 0;
|
||||
continue;
|
||||
}
|
||||
@ -556,11 +556,11 @@ AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const {
|
||||
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);
|
||||
if (backend == nullptr) {
|
||||
return RangeFinder_NotConnected;
|
||||
return Status::NotConnected;
|
||||
}
|
||||
return backend->status();
|
||||
}
|
||||
@ -589,7 +589,7 @@ AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) co
|
||||
AP_RangeFinder_Backend *backend = get_backend(i);
|
||||
if (backend != nullptr &&
|
||||
backend->orientation() == orientation &&
|
||||
backend->status() == RangeFinder_Good) {
|
||||
backend->status() == Status::Good) {
|
||||
return backend;
|
||||
}
|
||||
}
|
||||
|
@ -85,19 +85,19 @@ public:
|
||||
HYPERBOLA = 2
|
||||
};
|
||||
|
||||
enum RangeFinder_Status {
|
||||
RangeFinder_NotConnected = 0,
|
||||
RangeFinder_NoData,
|
||||
RangeFinder_OutOfRangeLow,
|
||||
RangeFinder_OutOfRangeHigh,
|
||||
RangeFinder_Good
|
||||
enum class Status {
|
||||
NotConnected = 0,
|
||||
NoData,
|
||||
OutOfRangeLow,
|
||||
OutOfRangeHigh,
|
||||
Good
|
||||
};
|
||||
|
||||
// The RangeFinder_State structure is filled in by the backend driver
|
||||
struct RangeFinder_State {
|
||||
uint16_t distance_cm; // distance: in cm
|
||||
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)
|
||||
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 ground_clearance_cm_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;
|
||||
uint8_t range_valid_count_orient(enum Rotation orientation) const;
|
||||
const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
|
||||
|
@ -38,18 +38,18 @@ MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const
|
||||
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) {
|
||||
// turned off at runtime?
|
||||
return RangeFinder::RangeFinder_NotConnected;
|
||||
return RangeFinder::Status::NotConnected;
|
||||
}
|
||||
return state.status;
|
||||
}
|
||||
|
||||
// true if sensor is returning data
|
||||
bool AP_RangeFinder_Backend::has_data() const {
|
||||
return ((state.status != RangeFinder::RangeFinder_NotConnected) &&
|
||||
(state.status != RangeFinder::RangeFinder_NoData));
|
||||
return ((state.status != RangeFinder::Status::NotConnected) &&
|
||||
(state.status != RangeFinder::Status::NoData));
|
||||
}
|
||||
|
||||
// update status based on distance measurement
|
||||
@ -57,21 +57,21 @@ void AP_RangeFinder_Backend::update_status()
|
||||
{
|
||||
// check distance
|
||||
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) {
|
||||
set_status(RangeFinder::RangeFinder_OutOfRangeLow);
|
||||
set_status(RangeFinder::Status::OutOfRangeLow);
|
||||
} else {
|
||||
set_status(RangeFinder::RangeFinder_Good);
|
||||
set_status(RangeFinder::Status::Good);
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
// update valid count
|
||||
if (_status == RangeFinder::RangeFinder_Good) {
|
||||
if (_status == RangeFinder::Status::Good) {
|
||||
if (state.range_valid_count < 10) {
|
||||
state.range_valid_count++;
|
||||
}
|
||||
|
@ -40,7 +40,7 @@ public:
|
||||
int16_t min_distance_cm() const { return params.min_distance_cm; }
|
||||
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
|
||||
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(); }
|
||||
|
||||
// true if sensor is returning data
|
||||
@ -62,7 +62,7 @@ protected:
|
||||
void update_status();
|
||||
|
||||
// set status and update valid_count
|
||||
void set_status(RangeFinder::RangeFinder_Status status);
|
||||
void set_status(RangeFinder::Status status);
|
||||
|
||||
RangeFinder::RangeFinder_State &state;
|
||||
AP_RangeFinder_Params ¶ms;
|
||||
|
Loading…
Reference in New Issue
Block a user