Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 09:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: average over 100Hz frames in USD1 CAN driver

Browse Source 
this reduces noise
This commit is contained in:
Andrew Tridgell 2021-10-18 14:20:53 +11:00 committed by Peter Barker
parent c56bd323ee
commit c1b9585926
2 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
View File

@ -17,24 +17,26 @@ AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State
void AP_RangeFinder_USD1_CAN::update(void)
{
WITH_SEMAPHORE(_sem);
if ((AP_HAL::millis() - _last_reading_ms) > 500) {
// if data is older than 500ms, report NoData
const uint32_t now = AP_HAL::millis();
if (_distance_count == 0 && now - state.last_reading_ms > 500) {
// no new data.
set_status(RangeFinder::Status::NoData);
} else if (new_data) {
state.distance_m = _distance_cm * 0.01f;
state.last_reading_ms = _last_reading_ms;
} else if (_distance_count != 0) {
state.distance_m = _distance_sum / _distance_count;
state.last_reading_ms = AP_HAL::millis();
_distance_sum = 0;
_distance_count = 0;
update_status();
new_data = false;
}
}
// handler for incoming frames
// handler for incoming frames. These come in at 100Hz
void AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
{
WITH_SEMAPHORE(_sem);
_distance_cm = (frame.data[0]<<8) | frame.data[1];
_last_reading_ms = AP_HAL::millis();
new_data = true;
const uint16_t dist_cm = (frame.data[0]<<8) | frame.data[1];
_distance_sum += dist_cm * 0.01;
_distance_count++;
}
#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS

5
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h
View File

@ -19,9 +19,8 @@ protected:
return MAV_DISTANCE_SENSOR_RADAR;
}
private:
bool new_data;
uint16_t _distance_cm;
uint32_t _last_reading_ms;
float _distance_sum;
uint32_t _distance_count;
};
#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS