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_BLHeli: index bi-dir dshot channels through the motor map

Browse Source 
error if no digital mask has been set for bi-dir dshot
This commit is contained in:
Andy Piper 2021-04-20 21:55:55 +01:00 committed by Andrew Tridgell
parent 0726870e6f
commit 9f5dbfb092
1 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -31,6 +31,7 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -1255,6 +1256,13 @@ void AP_BLHeli::update(void)
} }
} }
} }
// check the user hasn't goofed, this also prevents weird crashes on arming
if (!initialised && channel_mask.get() == 0 && channel_auto.get() == 0
&& (channel_bidir_dshot_mask.get() != 0 || channel_reversible_mask.get())) {
AP_BoardConfig::config_error("DSHOT needs SERVO_BLH_AUTO or _MASK");
}
if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) { if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) {
if (initialised && run_test.get() > 0) { if (initialised && run_test.get() > 0) {
run_connection_test(run_test.get() - 1); run_connection_test(run_test.get() - 1);
@ -1376,7 +1384,7 @@ float AP_BLHeli::get_average_motor_frequency_hz() const
// average the rpm of each motor as reported by BLHeli and convert to Hz // average the rpm of each motor as reported by BLHeli and convert to Hz
for (uint8_t i = 0; i < num_motors; i++) { for (uint8_t i = 0; i < num_motors; i++) {
if (has_bidir_dshot(i)) { if (has_bidir_dshot(i)) {
uint16_t erpm = hal.rcout->get_erpm(i); uint16_t erpm = hal.rcout->get_erpm(motor_map[i]);
if (erpm > 0 && erpm < 0xFFFF) { if (erpm > 0 && erpm < 0xFFFF) {
valid_escs++; valid_escs++;
motor_freq += (erpm * 200 / motor_poles) / 60.f; motor_freq += (erpm * 200 / motor_poles) / 60.f;
@ -1404,7 +1412,7 @@ uint8_t AP_BLHeli::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
// average the rpm of each motor as reported by BLHeli and convert to Hz // average the rpm of each motor as reported by BLHeli and convert to Hz
for (uint8_t i = 0; i < num_motors && i < nfreqs; i++) { for (uint8_t i = 0; i < num_motors && i < nfreqs; i++) {
if (has_bidir_dshot(i)) { if (has_bidir_dshot(i)) {
uint16_t erpm = hal.rcout->get_erpm(i); uint16_t erpm = hal.rcout->get_erpm(motor_map[i]);
if (erpm > 0 && erpm < 0xFFFF) { if (erpm > 0 && erpm < 0xFFFF) {
freqs[valid_escs++] = (erpm * 200 / motor_poles) / 60.f; freqs[valid_escs++] = (erpm * 200 / motor_poles) / 60.f;
} }
@ -1474,16 +1482,18 @@ void AP_BLHeli::read_telemetry_packet(void)
uint16_t trpm = td.rpm; uint16_t trpm = td.rpm;
float terr = 0.0f; float terr = 0.0f;
const uint8_t motor_idx = motor_map[last_telem_esc];
if (logger && logger->logging_enabled() if (logger && logger->logging_enabled()
// log at 10Hz // log at 10Hz
&& td.timestamp_ms - last_log_ms[last_telem_esc] > 100) { && td.timestamp_ms - last_log_ms[last_telem_esc] > 100) {
if (has_bidir_dshot(last_telem_esc)) { if (has_bidir_dshot(last_telem_esc)) {
const uint16_t erpm = hal.rcout->get_erpm(last_telem_esc); const uint16_t erpm = hal.rcout->get_erpm(motor_idx);
if (erpm != 0xFFFF) { // don't log invalid values if (erpm != 0xFFFF) { // don't log invalid values
trpm = erpm * 200 / motor_poles; trpm = erpm * 200 / motor_poles;
} }
terr = hal.rcout->get_erpm_error_rate(last_telem_esc); terr = hal.rcout->get_erpm_error_rate(motor_idx);
} }
logger->Write_ESC(uint8_t(last_telem_esc), logger->Write_ESC(uint8_t(last_telem_esc),
@ -1500,11 +1510,11 @@ void AP_BLHeli::read_telemetry_packet(void)
if (debug_level >= 2) { if (debug_level >= 2) {
if (has_bidir_dshot(last_telem_esc)) { if (has_bidir_dshot(last_telem_esc)) {
trpm = hal.rcout->get_erpm(last_telem_esc); trpm = hal.rcout->get_erpm(motor_idx);
if (trpm != 0xFFFF) { if (trpm != 0xFFFF) {
trpm = trpm * 200 / motor_poles; trpm = trpm * 200 / motor_poles;
} }
terr = hal.rcout->get_erpm_error_rate(last_telem_esc); terr = hal.rcout->get_erpm_error_rate(motor_idx);
} }
hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u e=%.1f t=%u\n", hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u e=%.1f t=%u\n",
last_telem_esc, last_telem_esc,
@ -1530,8 +1540,9 @@ void AP_BLHeli::log_bidir_telemetry(void)
&& now - last_log_ms[last_telem_esc] > 100) { && now - last_log_ms[last_telem_esc] > 100) {
if (has_bidir_dshot(last_telem_esc)) { if (has_bidir_dshot(last_telem_esc)) {
uint16_t trpm = hal.rcout->get_erpm(last_telem_esc); const uint8_t motor_idx = motor_map[last_telem_esc];
const float terr = hal.rcout->get_erpm_error_rate(last_telem_esc); uint16_t trpm = hal.rcout->get_erpm(motor_idx);
const float terr = hal.rcout->get_erpm_error_rate(motor_idx);
if (trpm != 0xFFFF) { // don't log invalid values as they are never used if (trpm != 0xFFFF) { // don't log invalid values as they are never used
trpm = trpm * 200 / motor_poles; trpm = trpm * 200 / motor_poles;