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

AP_BLHeli: ensure digital motor masks are setup correctly regardless of AUTO setting

This commit is contained in:
Andy Piper 2021-06-02 17:22:04 +01:00 committed by Andrew Tridgell
parent cf257074c6
commit 4a05e65367
1 changed files with 21 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -60,7 +60,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
// @Param: AUTO
// @DisplayName: BLHeli auto-enable for multicopter motors
// @DisplayName: BLHeli pass-thru auto-enable for multicopter motors
// @Description: If set to 1 this auto-enables BLHeli pass-thru support for all multicopter motors
// @Values: 0:Disabled,1:Enabled
// @User: Standard
@ -1270,19 +1270,10 @@ 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() != 0
|| channel_reversed_mask.get() != 0)) {
AP_BoardConfig::config_error("DSHOT needs SERVO_BLH_AUTO or _MASK");
}
if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) {
if (initialised && run_test.get() > 0) {
run_connection_test(run_test.get() - 1);
}
return;
}
}
@ -1296,15 +1287,18 @@ void AP_BLHeli::init(void)
run_test.set_and_notify(0);
if (last_control_port > 0 && last_control_port != control_port) {
gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+last_control_port), nullptr);
last_control_port = -1;
}
if (gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+control_port),
FUNCTOR_BIND_MEMBER(&AP_BLHeli::protocol_handler,
bool, uint8_t, AP_HAL::UARTDriver *))) {
debug("BLHeli installed on port %u", (unsigned)control_port);
last_control_port = control_port;
// only install pass-thru protocol handler if either auto or the motor mask are set
if (channel_mask.get() != 0 || channel_auto.get() != 0) {
if (last_control_port > 0 && last_control_port != control_port) {
gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+last_control_port), nullptr);
last_control_port = -1;
}
if (gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+control_port),
FUNCTOR_BIND_MEMBER(&AP_BLHeli::protocol_handler,
bool, uint8_t, AP_HAL::UARTDriver *))) {
debug("BLHeli installed on port %u", (unsigned)control_port);
last_control_port = control_port;
}
}
#if HAL_WITH_IO_MCU
@ -1364,20 +1358,18 @@ void AP_BLHeli::init(void)
/*
plane and copter can use AP_Motors to get an automatic mask
*/
if (channel_auto.get() == 1) {
#if APM_BUILD_TYPE(APM_BUILD_Rover)
AP_MotorsUGV *motors = AP::motors_ugv();
AP_MotorsUGV *motors = AP::motors_ugv();
#else
AP_Motors *motors = AP::motors();
AP_Motors *motors = AP::motors();
#endif
if (motors) {
uint16_t motormask = motors->get_motor_mask();
// set the rest of the digital channels
if (motors->get_pwm_type() >= AP_Motors::PWM_TYPE_DSHOT150) {
digital_mask |= motormask;
}
mask |= motormask;
if (motors) {
uint16_t motormask = motors->get_motor_mask();
// set the rest of the digital channels
if (motors->get_pwm_type() >= AP_Motors::PWM_TYPE_DSHOT150) {
digital_mask |= motormask;
}
mask |= motormask;
}
#endif
// tell SRV_Channels about ESC capabilities