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

AP_BLHeli: stop motors if conection lost in motor test

Browse Source
This commit is contained in:
Peter Hall 2020-01-12 21:44:39 +00:00 committed by Andrew Tridgell
parent 38f68c4eba
commit d5843ff03a
1 changed files with 22 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -40,6 +40,9 @@ extern const AP_HAL::HAL& hal;
// the MSP protocol on hal.console // the MSP protocol on hal.console
#define BLHELI_UART_LOCK_KEY 0x20180402 #define BLHELI_UART_LOCK_KEY 0x20180402
// if no packets are received for this time and motor control is active BLH will disconect (stoping motors)
#define MOTOR_ACTIVE_TIMEOUT 1000
const AP_Param::GroupInfo AP_BLHeli::var_info[] = { const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
// @Param: MASK // @Param: MASK
// @DisplayName: BLHeli Channel Bitmask // @DisplayName: BLHeli Channel Bitmask
@ -1183,10 +1186,18 @@ void AP_BLHeli::run_connection_test(uint8_t chan)
*/ */
void AP_BLHeli::update(void) void AP_BLHeli::update(void)
{ {
if (initialised && bool motor_control_active = false;
timeout_sec && for (uint8_t i = 0; i < num_motors; i++) {
uart_locked && bool reversed = ((1U<< motor_map[i]) & channel_reversible_mask.get()) != 0;
AP_HAL::millis() - last_valid_ms > uint32_t(timeout_sec.get())*1000U) { if (hal.rcout->read( motor_map[i]) != (reversed ? 1500 : 1000)) {
motor_control_active = true;
}
}
uint32_t now = AP_HAL::millis();
if (initialised && uart_locked &&
((timeout_sec && now - last_valid_ms > uint32_t(timeout_sec.get())*1000U) ||
(motor_control_active && now - last_valid_ms > MOTOR_ACTIVE_TIMEOUT))) {
// we're not processing requests any more, shutdown serial // we're not processing requests any more, shutdown serial
// output // output
if (serial_start_ms) { if (serial_start_ms) {
@ -1195,11 +1206,17 @@ void AP_BLHeli::update(void)
} }
if (motors_disabled) { if (motors_disabled) {
motors_disabled = false; motors_disabled = false;
SRV_Channels::set_disabled_channel_mask(0); SRV_Channels::set_disabled_channel_mask(0);
} }
debug("Unlocked UART"); debug("Unlocked UART");
uart->lock_port(0, 0); uart->lock_port(0, 0);
uart_locked = false; uart_locked = false;
if (motor_control_active) {
for (uint8_t i = 0; i < num_motors; i++) {
bool reversed = ((1U<<motor_map[i]) & channel_reversible_mask.get()) != 0;
hal.rcout->write(motor_map[i], reversed ? 1500 : 1000);
}
}
} }
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) {