Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Randy Mackay 2018-08-27 16:02:51 +09:00
parent 9e27b93538
commit 1b0f0a7559
30 changed files with 46 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
View File

@ -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

1
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
View File

@ -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();
}

4
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -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);
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -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;
};

4
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -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);
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -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;

1
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -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 {

4
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -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);
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -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;
};

6
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -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 {

1
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -30,7 +30,6 @@ protected:
private:
uint16_t distance_cm;
uint32_t last_update_ms;
// start a reading
static bool start_reading(void);

4
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -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);
}

1
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h
View File

@ -33,7 +33,6 @@ private:
uint16_t distance;
bool new_distance;
uint32_t last_update_ms;
// start a reading
bool start_reading(void);

4
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp
View File

@ -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);
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
View File

@ -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;
};

4
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp
View File

@ -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);
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h
View File

@ -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

8
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
View File

@ -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) {

1
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
View File

@ -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;

1
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -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;

3
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

@ -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();

1
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp
View File

@ -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();

4
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp
View File

@ -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;
}

1
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h
View File

@ -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;

3
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -115,7 +115,8 @@ void AP_RangeFinder_analog::update(void)
if (dist_m < 0) {
dist_m = 0;
}
state.distance_cm = dist_m * 100.0f;
state.distance_cm = dist_m * 100.0f;
state.last_reading_ms = AP_HAL::millis();
// update range_valid state based on distance measured
update_status();

4
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp
View File

@ -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);
}
}

1
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h
View File

@ -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;

9
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -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);

2
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -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

3
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -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