From 7663b8eade6499969b7fae117b1dd547ac195f00 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 13 Apr 2015 15:03:19 +0900 Subject: [PATCH] RangeFinder: replace healthy with status and no_data methods --- libraries/AP_RangeFinder/RangeFinder.cpp | 40 ++++++++++++++++--- libraries/AP_RangeFinder/RangeFinder.h | 31 +++++++++++--- .../AP_RangeFinder/RangeFinder_Backend.cpp | 28 +++++++++++++ .../AP_RangeFinder/RangeFinder_Backend.h | 9 ++++- 4 files changed, 97 insertions(+), 11 deletions(-) diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 6665559720..b97f72357f 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -207,6 +207,10 @@ void RangeFinder::init(void) state[i].pre_arm_check = false; state[i].pre_arm_distance_min = 9999; // initialise to an arbitrary large value state[i].pre_arm_distance_max = 0; + + // initialise status + state[i].status = RangeFinder_NotConnected; + state[i].range_valid_count = 0; } } @@ -220,7 +224,8 @@ void RangeFinder::update(void) if (drivers[i] != NULL) { if (_type[i] == RangeFinder_TYPE_NONE) { // allow user to disable a rangefinder at runtime - state[i].healthy = false; + state[i].status = RangeFinder_NotConnected; + state[i].range_valid_count = 0; continue; } drivers[i]->update(); @@ -228,9 +233,9 @@ void RangeFinder::update(void) } } - // work out primary instance - first healthy sensor + // work out primary instance - first sensor returning good data for (int8_t i=num_instances-1; i>=0; i--) { - if (drivers[i] != NULL && state[i].healthy) { + if (drivers[i] != NULL && (state[i].status == RangeFinder_Good)) { primary_instance = i; } } @@ -290,6 +295,31 @@ void RangeFinder::detect_instance(uint8_t instance) } } +// query status +RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const +{ + // sanity check instance + if (instance > RANGEFINDER_MAX_INSTANCES) { + return RangeFinder_NotConnected; + } + + if (drivers[instance] == NULL || _type[instance] == RangeFinder_TYPE_NONE) { + return RangeFinder_NotConnected; + } + + return state[instance].status; +} + +// true if sensor is returning data +bool RangeFinder::has_data(uint8_t instance) const +{ + // sanity check instance + if (instance > RANGEFINDER_MAX_INSTANCES) { + return RangeFinder_NotConnected; + } + return ((state[instance].status != RangeFinder_NotConnected) && (state[instance].status != RangeFinder_NoData)); +} + /* returns true if pre-arm checks have passed for all range finders these checks involve the user lifting or rotating the vehicle so that sensor readings between @@ -314,8 +344,8 @@ bool RangeFinder::pre_arm_check() const */ void RangeFinder::update_pre_arm_check(uint8_t instance) { - // return immediately if already passed or unhealty - if (state[instance].pre_arm_check || !state[instance].healthy) { + // return immediately if already passed or no sensor data + if (state[instance].pre_arm_check || state[instance].status == RangeFinder_NotConnected || state[instance].status == RangeFinder_NoData) { return; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 710cd09968..eb4e96f32c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -58,6 +58,13 @@ public: FUNCTION_HYPERBOLA = 2 }; + enum RangeFinder_Status { + RangeFinder_NotConnected = 0, + RangeFinder_NoData, + RangeFinder_OutOfRangeLow, + RangeFinder_OutOfRangeHigh, + RangeFinder_Good + }; // The RangeFinder_State structure is filled in by the backend driver struct RangeFinder_State { @@ -65,7 +72,8 @@ public: uint16_t distance_cm; // distance: in cm uint16_t voltage_mv; // voltage in millivolts, // if applicable, otherwise 0 - bool healthy; // sensor is communicating correctly + enum RangeFinder_Status status; // sensor status + uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10) bool pre_arm_check; // true if sensor has passed pre-arm checks uint16_t pre_arm_distance_min; // min distance captured during pre-arm checks uint16_t pre_arm_distance_max; // max distance captured during pre-arm checks @@ -135,11 +143,24 @@ public: return _ground_clearance_cm[primary_instance]; } - bool healthy(uint8_t instance) const { - return instance < num_instances && _RangeFinder_STATE(instance).healthy; + // query status + RangeFinder_Status status(uint8_t instance) const; + RangeFinder_Status status(void) const { + return status(primary_instance); } - bool healthy() const { - return healthy(primary_instance); + + // true if sensor is returning data + bool has_data(uint8_t instance) const; + bool has_data() const { + return has_data(primary_instance); + } + + // returns count of consecutive good readings + uint8_t range_valid_count() const { + return range_valid_count(primary_instance); + } + uint8_t range_valid_count(uint8_t instance) const { + return _RangeFinder_STATE(instance).range_valid_count; } /* diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index c694cac498..4254927310 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -28,3 +28,31 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t ins state(_state) { } + +// update status based on distance measurement +void AP_RangeFinder_Backend::update_status() +{ + // check distance + if (state.distance_cm > ranger._max_distance_cm[state.instance]) { + set_status(RangeFinder::RangeFinder_OutOfRangeHigh); + } else if (state.distance_cm < ranger._min_distance_cm[state.instance]) { + set_status(RangeFinder::RangeFinder_OutOfRangeLow); + } else { + set_status(RangeFinder::RangeFinder_Good); + } +} + +// set status and update valid count +void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status status) +{ + state.status = status; + + // update valid count + if (status == RangeFinder::RangeFinder_Good) { + if (state.range_valid_count < 10) { + state.range_valid_count++; + } + } else { + state.range_valid_count = 0; + } +} diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h index fc76aed060..90beee5f62 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h @@ -38,8 +38,15 @@ public: bool out_of_range(void) const { return ranger._powersave_range > 0 && ranger.estimated_terrain_height > ranger._powersave_range; } - + protected: + + // update status based on distance measurement + void update_status(); + + // set status and update valid_count + void set_status(RangeFinder::RangeFinder_Status status); + RangeFinder &ranger; RangeFinder::RangeFinder_State &state; };