mirror of https://github.com/ArduPilot/ardupilot
HAL_ChibiOS: always send zero DShot when disarmed
this prevents a misconfigured system (for example SERVOn_REVERSED=1) from running a motor while disarmed. See https://discuss.ardupilot.org/t/plane-4-1-0-beta/72434/34?u=tridge
This commit is contained in:
parent
455787869e
commit
7ac895db77
|
@ -1334,6 +1334,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
|
|||
}
|
||||
#endif
|
||||
bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
|
||||
bool armed = hal.util->get_soft_armed();
|
||||
|
||||
memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
|
||||
|
||||
|
@ -1383,6 +1384,11 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
|
|||
value += 47;
|
||||
}
|
||||
|
||||
if (!armed) {
|
||||
// when disarmed we always send a zero value
|
||||
value = 0;
|
||||
}
|
||||
|
||||
// according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc
|
||||
bool request_telemetry = telem_request_mask & chan_mask;
|
||||
uint16_t packet = create_dshot_packet(value, request_telemetry, group.bdshot.enabled);
|
||||
|
|
Loading…
Reference in New Issue