Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ESC_Telem: add method to get average motor RPM

Browse Source
This commit is contained in:
Michel Pastor 2021-07-06 00:31:40 +02:00 committed by Andrew Tridgell
parent e9f0c59e61
commit f9b149e793
2 changed files with 17 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -34,39 +34,28 @@ AP_ESC_Telem::AP_ESC_Telem()
_singleton = this; _singleton = this;
} }
// return the average motor frequency in Hz for selected motors // return the average motor RPM
float AP_ESC_Telem::get_average_motor_frequency_hz(uint32_t servo_channel_mask) const float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const
{ {
float motor_freq = 0.0f; float rpm_avg = 0.0f;
uint8_t valid_escs = 0; uint8_t valid_escs = 0;
// average the rpm of each motor and convert to Hz // average the rpm of each motor
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
if (BIT_IS_SET(servo_channel_mask,i)) { if (BIT_IS_SET(servo_channel_mask,i)) {
float rpm; float rpm;
if (get_rpm(i, rpm)) { if (get_rpm(i, rpm)) {
motor_freq += rpm * (1.0f / 60.0f); rpm_avg += rpm;
valid_escs++; valid_escs++;
} }
} }
} }
if (valid_escs > 0) { if (valid_escs > 0) {
motor_freq /= valid_escs; rpm_avg /= valid_escs;
} }
return motor_freq; return rpm_avg;
}
// 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 // return all the motor frequencies in Hz for dynamic filtering

15
libraries/AP_ESC_Telem/AP_ESC_Telem.h
View File

@ -27,6 +27,12 @@ public:
// get an individual ESC's raw rpm if available // get an individual ESC's raw rpm if available
bool get_raw_rpm(uint8_t esc_index, float& rpm) const; bool get_raw_rpm(uint8_t esc_index, float& rpm) const;
// return the average motor RPM
float get_average_motor_rpm(uint32_t servo_channel_mask) const;
// return the average motor RPM
float get_average_motor_rpm() const { return get_average_motor_rpm(0xFFFFFFFF); }
// get an individual ESC's temperature in centi-degrees if available, returns true on success // get an individual ESC's temperature in centi-degrees if available, returns true on success
bool get_temperature(uint8_t esc_index, int16_t& temp) const; bool get_temperature(uint8_t esc_index, int16_t& temp) const;
@ -45,14 +51,11 @@ public:
// get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success // 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; bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;
// return the average selected motor rpm // return the average motor frequency in Hz for dynamic filtering
float get_average_motor_rpm(uint32_t esc_mask) const; float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };
// return the average motor frequency in Hz for dynamic filtering // return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz(uint32_t esc_mask) const; float get_average_motor_frequency_hz() const { return get_average_motor_frequency_hz(0xFFFFFFFF); }
// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz() const;
// return all of the motor frequencies in Hz for dynamic filtering // return all of the motor frequencies in Hz for dynamic filtering
uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const; uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;