mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Periph: limit peripheral mag to 25Hz by default
we only read mag on the vehicles at 10Hz. Some magnetometers were reporting data at 500Hz (see MMC5xx3 driver) by sampling more slowly the data is accumulated and averaged on the node which saves a lot of bandwidth
This commit is contained in:
parent
3c65b5e5a0
commit
2781f65d56
@ -73,6 +73,10 @@ extern AP_Periph_FW periph;
|
|||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_PERIPH_MAG_MAX_RATE
|
||||||
|
#define AP_PERIPH_MAG_MAX_RATE 25U
|
||||||
|
#endif
|
||||||
|
|
||||||
#define DEBUG_PRINTS 0
|
#define DEBUG_PRINTS 0
|
||||||
#define DEBUG_PKTS 0
|
#define DEBUG_PKTS 0
|
||||||
#if DEBUG_PRINTS
|
#if DEBUG_PRINTS
|
||||||
@ -1777,6 +1781,15 @@ void AP_Periph_FW::can_mag_update(void)
|
|||||||
if (!compass.available()) {
|
if (!compass.available()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if AP_PERIPH_MAG_MAX_RATE > 0
|
||||||
|
// don't flood the bus with very high rate magnetometers
|
||||||
|
const uint32_t now_ms = AP_HAL::millis();
|
||||||
|
if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
compass.read();
|
compass.read();
|
||||||
#if AP_PERIPH_PROBE_CONTINUOUS
|
#if AP_PERIPH_PROBE_CONTINUOUS
|
||||||
if (compass.get_count() == 0) {
|
if (compass.get_count() == 0) {
|
||||||
|
Loading…
Reference in New Issue
Block a user