mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 16:08:28 -04:00
AP_RangeFinder: use enum-class for Status
This commit is contained in:
parent
1989decbc1
commit
82db4383d4
@ -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();
|
||||||
}
|
}
|
||||||
|
@ -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) {
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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:
|
||||||
|
@ -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;
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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) {
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
|
@ -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++;
|
||||||
}
|
}
|
||||||
|
@ -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 ¶ms;
|
AP_RangeFinder_Params ¶ms;
|
||||||
|
Loading…
Reference in New Issue
Block a user