aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DroneCAN: prevent invalid numbers spinning ESCs

Browse Source 
This prevents bugs and misconfigurations from causing DroneCAN ESCs to
spin at full speed while the vehicle is disarmed.
This commit is contained in:
Bob Long 2024-07-22 23:49:22 +10:00 committed by Andrew Tridgell
parent dcc526ec19
commit 8e707ef4e2
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -709,14 +709,17 @@ void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, con
int16_t AP_DroneCAN::scale_esc_output(uint8_t idx){
static const int16_t cmd_max = ((1<<13)-1);
float scaled = 0;
float scaled = hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse);
// Prevent invalid values (from misconfigured scaling parameters) from sending non-zero commands
if (!isfinite(scaled)) {
return 0;
}
scaled = constrain_float(scaled, -1, 1);
//Check if this channel has a reversible ESC. If it does, we can send negative commands.
if ((((uint32_t) 1) << idx) & _esc_rv) {
scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse));
scaled *= cmd_max;
} else {
scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[idx].pulse) + 1.0) / 2.0;
scaled = constrain_float(scaled, 0, cmd_max);
scaled = cmd_max * (scaled + 1.0) / 2.0;
}
return static_cast<int16_t>(scaled);