Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_ESC_Telem: added methods to get average rpm data by motor mask

Browse Source
This commit is contained in:
yaapu 2021-06-03 09:27:27 +02:00 committed by Peter Barker
parent 326b65c7ad
commit b4d54cf565
2 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -34,18 +34,20 @@ AP_ESC_Telem::AP_ESC_Telem()
_singleton = this;
}
// return the average motor frequency in Hz for dynamic filtering
float AP_ESC_Telem::get_average_motor_frequency_hz() const
// return the average motor frequency in Hz for selected motors
float AP_ESC_Telem::get_average_motor_frequency_hz(uint32_t servo_channel_mask) const
{
float motor_freq = 0.0f;
uint8_t valid_escs = 0;
// average the rpm of each motor and convert to Hz
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
float rpm;
if (get_rpm(i, rpm)) {
motor_freq += rpm * (1.0f / 60.0f);
valid_escs++;
if (BIT_IS_SET(servo_channel_mask,i)) {
float rpm;
if (get_rpm(i, rpm)) {
motor_freq += rpm * (1.0f / 60.0f);
valid_escs++;
}
}
}
if (valid_escs > 0) {
@ -55,6 +57,18 @@ float AP_ESC_Telem::get_average_motor_frequency_hz() const
return motor_freq;
}
// return the average motor frequency in Hz for dynamic filtering
float AP_ESC_Telem::get_average_motor_frequency_hz() const
{
return get_average_motor_frequency_hz(0xFFFFFFFF);
}
// return the average motor rpm for selected motor channels
float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const
{
return get_average_motor_frequency_hz(servo_channel_mask)*60.f; // Hz to rpm
}
// return all the motor frequencies in Hz for dynamic filtering
uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
{

6
libraries/AP_ESC_Telem/AP_ESC_Telem.h
View File

@ -45,6 +45,12 @@ public:
// get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;
// return the average selected motor rpm
float get_average_motor_rpm(uint32_t esc_mask) const;
// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz(uint32_t esc_mask) const;
// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz() const;