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

Some fixes for AP_BLHeli 

Added ACK_I_INVALID_CHANNEL as response for cmd_DeviceReset cmd_DeviceInitFlash on bad channel selection
Fixed params for MSP_MOTOR_CONFIG
Adapted some timeouts
This commit is contained in:
4712 2020-10-18 19:48:51 +02:00 committed by Andrew Tridgell
parent 59c5bbc8b8
commit 6956abdb45
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -423,10 +423,15 @@ void AP_BLHeli::msp_process_command(void)
case MSP_MOTOR_CONFIG: {
debug("MSP_MOTOR_CONFIG");
uint8_t buf[6];
uint8_t buf[10];
putU16(&buf[0], 1030); // min throttle
putU16(&buf[2], 2000); // max throttle
putU16(&buf[4], 1000); // min command
// API 1.42
buf[6] = num_motors; // motorCount
buf[7] = motor_poles; // motorPoleCount
buf[8] = 0; // useDshotTelemetry
buf[9] = 0; // FEATURE_ESC_SENSOR
msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
break;
}
@ -827,7 +832,7 @@ bool AP_BLHeli::BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint
uint8_t AP_BLHeli::BL_WriteFlash(const uint8_t *buf, uint16_t n)
{
return BL_WriteA(CMD_PROG_FLASH, buf, n, 250);
return BL_WriteA(CMD_PROG_FLASH, buf, n, 500);
}
bool AP_BLHeli::BL_VerifyFlash(const uint8_t *buf, uint16_t n)
@ -917,7 +922,7 @@ void AP_BLHeli::blheli_process_command(void)
debug("cmd_DeviceReset(%u)", unsigned(blheli.buf[0]));
if (blheli.buf[0] >= num_motors) {
debug("bad reset channel %u", blheli.buf[0]);
blheli.ack = ACK_D_GENERAL_ERROR;
blheli.ack = ACK_I_INVALID_CHANNEL;
blheli_send_reply(&blheli.buf[0], 1);
break;
}
@ -940,6 +945,8 @@ void AP_BLHeli::blheli_process_command(void)
debug("cmd_DeviceInitFlash(%u)", unsigned(blheli.buf[0]));
if (blheli.buf[0] >= num_motors) {
debug("bad channel %u", blheli.buf[0]);
blheli.ack = ACK_I_INVALID_CHANNEL;
blheli_send_reply(&blheli.buf[0], 1);
break;
}
blheli.chan = blheli.buf[0];
@ -1070,7 +1077,7 @@ void AP_BLHeli::blheli_process_command(void)
debug("cmd_DeviceWriteEEprom n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]);
switch (blheli.interface_mode[blheli.chan]) {
case imATM_BLB:
BL_WriteA(CMD_PROG_EEPROM, buf, nbytes, 1000);
BL_WriteA(CMD_PROG_EEPROM, buf, nbytes, 3000);
break;
default:
blheli.ack = ACK_D_GENERAL_ERROR;

1
libraries/AP_BLHeli/AP_BLHeli.h
View File

@ -282,6 +282,7 @@ private:
bool msp_process_byte(uint8_t c);
void blheli_crc_update(uint8_t c);
bool blheli_4way_process_byte(uint8_t c);
void msp_send_ack(uint8_t cmd);
void msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len);
void putU16(uint8_t *b, uint16_t v);
uint16_t getU16(const uint8_t *b);