mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder: support last_reading_ms
Benewake, LeddarOne, LightWareSerial, MAVLink, MaxsonarI2CXL, MaxsonarSerialLV, NMEA, PX4_PWM, uLanding and Wasp already stored the last read time so for these drivers, this change just moves that storage to the state structure analog, BBB_PRU, Bebop, LightWareI2C, PulsedLightLRF, TeraRangerI2C, VL53L0X did not store the last read time so this was added
This commit is contained in:
parent
9e27b93538
commit
1b0f0a7559
|
@ -116,5 +116,6 @@ void AP_RangeFinder_BBB_PRU::update(void)
|
|||
{
|
||||
state.status = (RangeFinder::RangeFinder_Status)rangerpru->status;
|
||||
state.distance_cm = rangerpru->distance;
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
}
|
||||
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
||||
|
|
|
@ -267,6 +267,7 @@ void AP_RangeFinder_Bebop::update(void)
|
|||
}
|
||||
|
||||
state.distance_cm = (uint16_t) (_altitude * 100);
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
}
|
||||
|
||||
|
|
|
@ -152,14 +152,14 @@ void AP_RangeFinder_Benewake::update(void)
|
|||
bool signal_ok;
|
||||
if (get_reading(state.distance_cm, signal_ok)) {
|
||||
// update range_valid state based on distance measured
|
||||
last_reading_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
if (signal_ok) {
|
||||
update_status();
|
||||
} else {
|
||||
// if signal is weak set status to out-of-range
|
||||
set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
|
||||
}
|
||||
} else if (AP_HAL::millis() - last_reading_ms > 200) {
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -39,7 +39,6 @@ private:
|
|||
|
||||
AP_HAL::UARTDriver *uart = nullptr;
|
||||
benewake_model_type model_type;
|
||||
uint32_t last_reading_ms;
|
||||
char linebuf[10];
|
||||
uint8_t linebuf_len;
|
||||
};
|
||||
|
|
|
@ -128,9 +128,9 @@ void AP_RangeFinder_LeddarOne::update(void)
|
|||
{
|
||||
if (get_reading(state.distance_cm)) {
|
||||
// update range_valid state based on distance measured
|
||||
last_reading_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - last_reading_ms > 200) {
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -69,7 +69,6 @@ private:
|
|||
LeddarOne_Status parse_response(uint8_t &number_detections);
|
||||
|
||||
AP_HAL::UARTDriver *uart = nullptr;
|
||||
uint32_t last_reading_ms;
|
||||
uint32_t last_sending_request_ms;
|
||||
uint32_t last_available_ms;
|
||||
|
||||
|
|
|
@ -97,6 +97,7 @@ void AP_RangeFinder_LightWareI2C::update(void)
|
|||
void AP_RangeFinder_LightWareI2C::timer(void)
|
||||
{
|
||||
if (get_reading(state.distance_cm)) {
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
// update range_valid state based on distance measured
|
||||
update_status();
|
||||
} else {
|
||||
|
|
|
@ -90,9 +90,9 @@ void AP_RangeFinder_LightWareSerial::update(void)
|
|||
{
|
||||
if (get_reading(state.distance_cm)) {
|
||||
// update range_valid state based on distance measured
|
||||
last_reading_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - last_reading_ms > 200) {
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -29,7 +29,6 @@ private:
|
|||
bool get_reading(uint16_t &reading_cm);
|
||||
|
||||
AP_HAL::UARTDriver *uart = nullptr;
|
||||
uint32_t last_reading_ms = 0;
|
||||
char linebuf[10];
|
||||
uint8_t linebuf_len = 0;
|
||||
};
|
||||
|
|
|
@ -28,7 +28,7 @@ extern const AP_HAL::HAL& hal;
|
|||
AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder::RangeFinder_State &_state) :
|
||||
AP_RangeFinder_Backend(_state)
|
||||
{
|
||||
last_update_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
distance_cm = 0;
|
||||
}
|
||||
|
||||
|
@ -53,7 +53,7 @@ void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg)
|
|||
|
||||
// only accept distances for downward facing sensors
|
||||
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
|
||||
last_update_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
distance_cm = packet.current_distance;
|
||||
}
|
||||
sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
|
||||
|
@ -66,7 +66,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() - last_update_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
|
||||
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
state.distance_cm = 0;
|
||||
} else {
|
||||
|
|
|
@ -30,7 +30,6 @@ protected:
|
|||
|
||||
private:
|
||||
uint16_t distance_cm;
|
||||
uint32_t last_update_ms;
|
||||
|
||||
// start a reading
|
||||
static bool start_reading(void);
|
||||
|
|
|
@ -133,7 +133,7 @@ void AP_RangeFinder_MaxsonarI2CXL::_timer(void)
|
|||
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
distance = d;
|
||||
new_distance = true;
|
||||
last_update_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
_sem->give();
|
||||
}
|
||||
}
|
||||
|
@ -149,7 +149,7 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void)
|
|||
state.distance_cm = distance;
|
||||
new_distance = false;
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - last_update_ms > 300) {
|
||||
} 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);
|
||||
}
|
||||
|
|
|
@ -33,7 +33,6 @@ private:
|
|||
|
||||
uint16_t distance;
|
||||
bool new_distance;
|
||||
uint32_t last_update_ms;
|
||||
|
||||
// start a reading
|
||||
bool start_reading(void);
|
||||
|
|
|
@ -94,9 +94,9 @@ void AP_RangeFinder_MaxsonarSerialLV::update(void)
|
|||
{
|
||||
if (get_reading(state.distance_cm)) {
|
||||
// update range_valid state based on distance measured
|
||||
last_reading_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - last_reading_ms > 500) {
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 500) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -29,7 +29,6 @@ private:
|
|||
bool get_reading(uint16_t &reading_cm);
|
||||
|
||||
AP_HAL::UARTDriver *uart = nullptr;
|
||||
uint32_t last_reading_ms = 0;
|
||||
char linebuf[10];
|
||||
uint8_t linebuf_len = 0;
|
||||
};
|
||||
|
|
|
@ -48,9 +48,9 @@ void AP_RangeFinder_NMEA::update(void)
|
|||
uint32_t now = AP_HAL::millis();
|
||||
if (get_reading(state.distance_cm)) {
|
||||
// update range_valid state based on distance measured
|
||||
_last_reading_ms = now;
|
||||
state.last_reading_ms = now;
|
||||
update_status();
|
||||
} else if ((now - _last_reading_ms) > 3000) {
|
||||
} else if ((now - state.last_reading_ms) > 3000) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -64,7 +64,6 @@ private:
|
|||
static int16_t char_to_hex(char a);
|
||||
|
||||
AP_HAL::UARTDriver *uart = nullptr; // pointer to serial uart
|
||||
uint32_t _last_reading_ms; // system time of last successful reading
|
||||
|
||||
// message decoding related members
|
||||
char _term[15]; // buffer for the current term within the current sentence
|
||||
|
|
|
@ -114,7 +114,7 @@ void AP_RangeFinder_PX4_PWM::update(void)
|
|||
// pulse widths in the log
|
||||
state.voltage_mv = pwm.pulse_width;
|
||||
|
||||
_last_pulse_time_ms = now;
|
||||
state.last_reading_ms = now;
|
||||
|
||||
// setup for scaling in meters per millisecond
|
||||
float _distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset;
|
||||
|
@ -162,9 +162,9 @@ void AP_RangeFinder_PX4_PWM::update(void)
|
|||
probably dead. Try resetting it. Tests show the sensor takes
|
||||
about 0.2s to boot, so 500ms offers some safety margin
|
||||
*/
|
||||
if (now - _last_pulse_time_ms > 500U && _disable_time_ms == 0) {
|
||||
if (now - state.last_reading_ms > 500U && _disable_time_ms == 0) {
|
||||
ioctl(_fd, SENSORIOCRESET, 0);
|
||||
_last_pulse_time_ms = now;
|
||||
state.last_reading_ms = now;
|
||||
|
||||
// if a stop pin is configured then disable the sensor for the
|
||||
// settle time
|
||||
|
@ -185,7 +185,7 @@ void AP_RangeFinder_PX4_PWM::update(void)
|
|||
(now - _disable_time_ms > settle_time_ms)) {
|
||||
hal.gpio->write(stop_pin, true);
|
||||
_disable_time_ms = 0;
|
||||
_last_pulse_time_ms = now;
|
||||
state.last_reading_ms = now;
|
||||
}
|
||||
|
||||
if (count != 0) {
|
||||
|
|
|
@ -41,7 +41,6 @@ protected:
|
|||
private:
|
||||
int _fd;
|
||||
uint64_t _last_timestamp;
|
||||
uint64_t _last_pulse_time_ms;
|
||||
uint32_t _disable_time_ms;
|
||||
uint32_t _good_sample_count;
|
||||
float _last_sample_distance_cm;
|
||||
|
|
|
@ -100,6 +100,7 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
|
|||
// remove momentary spikes
|
||||
if (abs(_distance_cm - last_distance_cm) < 100) {
|
||||
state.distance_cm = _distance_cm;
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
}
|
||||
last_distance_cm = _distance_cm;
|
||||
|
|
|
@ -160,7 +160,7 @@ void AP_RangeFinder_TeraRangerI2C::timer(void)
|
|||
uint16_t _distance_cm = 0;
|
||||
|
||||
if (collect_raw(_raw_distance) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if(process_raw_measure(_raw_distance, _distance_cm)){
|
||||
if (process_raw_measure(_raw_distance, _distance_cm)){
|
||||
accum.sum += _distance_cm;
|
||||
accum.count++;
|
||||
}
|
||||
|
@ -178,6 +178,7 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
|
|||
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if (accum.count > 0) {
|
||||
state.distance_cm = accum.sum / accum.count;
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
accum.sum = 0;
|
||||
accum.count = 0;
|
||||
update_status();
|
||||
|
|
|
@ -768,6 +768,7 @@ void AP_RangeFinder_VL53L0X::update(void)
|
|||
{
|
||||
if (counter > 0) {
|
||||
state.distance_cm = sum_mm / (10*counter);
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
sum_mm = 0;
|
||||
counter = 0;
|
||||
update_status();
|
||||
|
|
|
@ -103,7 +103,7 @@ bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
|
|||
if (c == '\n') {
|
||||
linebuf[linebuf_len] = 0;
|
||||
linebuf_len = 0;
|
||||
last_reading_ms = AP_HAL::millis();
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
if (isalpha(linebuf[0])) {
|
||||
parse_response();
|
||||
} else {
|
||||
|
@ -144,7 +144,7 @@ void AP_RangeFinder_Wasp::update(void) {
|
|||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
|
||||
if (AP_HAL::millis() - last_reading_ms > 500) {
|
||||
if (AP_HAL::millis() - state.last_reading_ms > 500) {
|
||||
// attempt to reconfigure on the assumption this was a bad baud setting
|
||||
configuration_state = WASP_CFG_RATE;
|
||||
}
|
||||
|
|
|
@ -49,7 +49,6 @@ private:
|
|||
void parse_response(void);
|
||||
|
||||
AP_HAL::UARTDriver *uart;
|
||||
uint32_t last_reading_ms;
|
||||
char linebuf[10];
|
||||
uint8_t linebuf_len;
|
||||
AP_Int16 mavg;
|
||||
|
|
|
@ -116,6 +116,7 @@ void AP_RangeFinder_analog::update(void)
|
|||
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();
|
||||
|
|
|
@ -211,10 +211,10 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm)
|
|||
void AP_RangeFinder_uLanding::update(void)
|
||||
{
|
||||
if (get_reading(state.distance_cm)) {
|
||||
state.last_reading_ms = AP_HAL::millis();
|
||||
// update range_valid state based on distance measured
|
||||
_last_reading_ms = AP_HAL::millis();
|
||||
update_status();
|
||||
} else if (AP_HAL::millis() - _last_reading_ms > 200) {
|
||||
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
|
||||
set_status(RangeFinder::RangeFinder_NoData);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -34,7 +34,6 @@ private:
|
|||
AP_HAL::UARTDriver *uart;
|
||||
uint8_t _linebuf[6];
|
||||
uint8_t _linebuf_len;
|
||||
uint32_t _last_reading_ms;
|
||||
bool _version_known;
|
||||
uint8_t _header;
|
||||
uint8_t _version;
|
||||
|
|
|
@ -910,6 +910,15 @@ const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) co
|
|||
return backend->get_pos_offset();
|
||||
}
|
||||
|
||||
uint32_t RangeFinder::last_reading_ms(enum Rotation orientation) const
|
||||
{
|
||||
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
||||
if (backend == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
return backend->last_reading_ms();
|
||||
}
|
||||
|
||||
MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotation orientation) const
|
||||
{
|
||||
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
||||
|
|
|
@ -87,6 +87,7 @@ public:
|
|||
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
|
||||
uint32_t last_reading_ms; // system time of last successful update from sensor
|
||||
|
||||
AP_Int8 type;
|
||||
AP_Int8 pin;
|
||||
|
@ -147,6 +148,7 @@ public:
|
|||
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;
|
||||
uint32_t last_reading_ms(enum Rotation orientation) const;
|
||||
|
||||
/*
|
||||
set an externally estimated terrain height. Used to enable power
|
||||
|
|
|
@ -55,6 +55,9 @@ public:
|
|||
// in metres relative to the body frame origin
|
||||
const Vector3f &get_pos_offset() const { return state.pos_offset; }
|
||||
|
||||
// return system time of last successful read from the sensor (used for DPTH logging)
|
||||
uint32_t last_reading_ms() const { return state.last_reading_ms; }
|
||||
|
||||
protected:
|
||||
|
||||
// update status based on distance measurement
|
||||
|
|
Loading…
Reference in New Issue