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

AP_Periph: added RNGFND_MAX_RATE parameter 

prevent duplicate samples and allow setting of maximum rate we sample
rangefinder
This commit is contained in:
Andrew Tridgell 2022-05-28 09:27:39 +10:00
parent 998072f600
commit 25d231187f
4 changed files with 21 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/AP_Periph/AP_Periph.h
View File

@ -174,6 +174,7 @@ public:
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
RangeFinder rangefinder; RangeFinder rangefinder;
uint32_t last_sample_ms;
#endif #endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT

9
Tools/AP_Periph/Parameters.cpp
View File

@ -276,6 +276,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
// @RebootRequired: True // @RebootRequired: True
GSCALAR(rangefinder_port, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT), GSCALAR(rangefinder_port, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
// @Param: RNGFND_MAX_RATE
// @DisplayName: Rangefinder max rate
// @Description: This is the maximum rate we send rangefinder data in Hz. Zero means no limit
// @Units: Hz
// @Range: 0 200
// @Increment: 1
// @User: Advanced
GSCALAR(rangefinder_max_rate, "RNGFND_MAX_RATE", 50),
// Rangefinder driver // Rangefinder driver
// @Group: RNGFND // @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp

2
Tools/AP_Periph/Parameters.h
View File

@ -55,6 +55,7 @@ public:
k_param_can_fdbaudrate0, k_param_can_fdbaudrate0,
k_param_can_fdbaudrate1, k_param_can_fdbaudrate1,
k_param_node_stats, k_param_node_stats,
k_param_rangefinder_max_rate,
}; };
AP_Int16 format_version; AP_Int16 format_version;
@ -81,6 +82,7 @@ public:
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
AP_Int32 rangefinder_baud; AP_Int32 rangefinder_baud;
AP_Int8 rangefinder_port; AP_Int8 rangefinder_port;
AP_Int16 rangefinder_max_rate;
#endif #endif
#ifdef HAL_PERIPH_ENABLE_ADSB #ifdef HAL_PERIPH_ENABLE_ADSB

11
Tools/AP_Periph/can.cpp
View File

@ -2239,8 +2239,9 @@ void AP_Periph_FW::can_rangefinder_update(void)
#endif #endif
uint32_t now = AP_HAL::native_millis(); uint32_t now = AP_HAL::native_millis();
static uint32_t last_update_ms; static uint32_t last_update_ms;
if (now - last_update_ms < 20) { if (g.rangefinder_max_rate > 0 &&
// max 50Hz data now - last_update_ms < 1000/g.rangefinder_max_rate) {
// limit to max rate
return; return;
} }
last_update_ms = now; last_update_ms = now;
@ -2250,6 +2251,12 @@ void AP_Periph_FW::can_rangefinder_update(void)
// don't send any data // don't send any data
return; return;
} }
const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE);
if (last_sample_ms == sample_ms) {
return;
}
last_sample_ms = sample_ms;
uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE); uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
uavcan_equipment_range_sensor_Measurement pkt {}; uavcan_equipment_range_sensor_Measurement pkt {};
pkt.sensor_id = rangefinder.get_address(0); pkt.sensor_id = rangefinder.get_address(0);