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

AP_Bootloader: speed up DroneCAN fw update

Browse Source 
pipeline file read requests to reduce impact of transport latency
This commit is contained in:
Andrew Tridgell 2023-07-31 16:26:43 +10:00
parent 23811af626
commit f353ae4933
3 changed files with 213 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

262
Tools/AP_Bootloader/can.cpp
View File

@ -47,18 +47,20 @@ static CANConfig cancfg = {
CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
0 // filled in below
};
// pipelining is not faster when using ChibiOS CAN driver
#define FW_UPDATE_PIPELINE_LEN 1
#else
static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
#endif
#ifndef CAN_APP_VERSION_MAJOR
#define CAN_APP_VERSION_MAJOR 1
#define CAN_APP_VERSION_MAJOR 2
#endif
#ifndef CAN_APP_VERSION_MINOR
#define CAN_APP_VERSION_MINOR 0
#endif
#ifndef CAN_APP_NODE_NAME
#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph"
#define CAN_APP_NODE_NAME "org.ardupilot." CHIBIOS_BOARD_NAME
#endif
static uint8_t node_id_allocation_transfer_id;
@ -66,14 +68,30 @@ static uavcan_protocol_NodeStatus node_status;
static uint32_t send_next_node_id_allocation_request_at_ms;
static uint8_t node_id_allocation_unique_id_offset;
static void processTx(void);
// keep up to 4 transfers in progress
#ifndef FW_UPDATE_PIPELINE_LEN
#define FW_UPDATE_PIPELINE_LEN 4
#endif
static struct {
uint64_t ofs;
uint32_t last_ms;
uint32_t rtt_ms;
uint32_t ofs;
uint8_t node_id;
uint8_t transfer_id;
uint8_t path[sizeof(uavcan_protocol_file_Path::path.data)+1];
uint8_t sector;
uint32_t sector_ofs;
uint8_t transfer_id;
uint8_t idx;
struct {
uint8_t tx_id;
uint32_t sent_ms;
uint32_t offset;
bool have_reply;
uavcan_protocol_file_ReadResponse pkt;
} reads[FW_UPDATE_PIPELINE_LEN];
uint16_t erased_to;
} fw_update;
/*
@ -150,32 +168,83 @@ static void handle_get_node_info(CanardInstance* ins,
/*
send a read for a fw update
*/
static void send_fw_read(void)
static bool send_fw_read(uint8_t idx)
{
uint32_t now = AP_HAL::millis();
if (now - fw_update.last_ms < 750) {
// the server may still be responding
return;
}
fw_update.last_ms = now;
auto &r = fw_update.reads[idx];
r.tx_id = fw_update.transfer_id;
r.have_reply = false;
uavcan_protocol_file_ReadRequest pkt {};
pkt.offset = fw_update.ofs;
pkt.path.path.len = strlen((const char *)fw_update.path);
pkt.offset = r.offset;
memcpy(pkt.path.path.data, fw_update.path, pkt.path.path.len);
uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE];
uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer, true);
canardRequestOrRespond(&canard,
fw_update.node_id,
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
UAVCAN_PROTOCOL_FILE_READ_ID,
&fw_update.transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardRequest,
&buffer[0],
total_size);
if (canardRequestOrRespond(&canard,
fw_update.node_id,
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
UAVCAN_PROTOCOL_FILE_READ_ID,
&fw_update.transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardRequest,
&buffer[0],
total_size) > 0) {
// mark it as having been sent
r.sent_ms = AP_HAL::millis();
return true;
}
return false;
}
/*
send a read for a fw update
*/
static void send_fw_reads(void)
{
const uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<FW_UPDATE_PIPELINE_LEN; i++) {
const uint8_t idx = (fw_update.idx+i) % FW_UPDATE_PIPELINE_LEN;
const auto &r = fw_update.reads[idx];
if (r.have_reply) {
continue;
}
if (r.sent_ms != 0 && now - r.sent_ms < 10+2*MAX(250,fw_update.rtt_ms)) {
// waiting on a response
continue;
}
if (!send_fw_read(idx)) {
break;
}
}
}
/*
erase up to at least the given sector number
*/
static void erase_to(uint16_t sector)
{
if (sector < fw_update.erased_to) {
return;
}
flash_func_erase_sector(sector);
fw_update.erased_to = sector+1;
/*
pre-erase any non-erased pages up to end of flash. This puts all
the load of erasing at the start of flashing which is much
faster than flashing as we go on boards with small flash
sectors. We stop at the first already erased page so we don't
end up wasting time erasing already erased pages when the
firmware is much smaller than the total flash size
*/
while (flash_func_sector_size(fw_update.erased_to) != 0 &&
!flash_func_is_erased(fw_update.erased_to)) {
flash_func_erase_sector(fw_update.erased_to);
fw_update.erased_to++;
}
}
/*
@ -183,51 +252,100 @@ static void send_fw_read(void)
*/
static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer)
{
const uint8_t tx_id = fw_update.transfer_id;
if ((transfer->transfer_id+1)%256 != tx_id ||
transfer->source_node_id != fw_update.node_id) {
if (transfer->source_node_id != fw_update.node_id) {
return;
}
uavcan_protocol_file_ReadResponse pkt;
if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) {
return;
}
const uint16_t len = pkt.data.len;
const uint16_t len_words = (len+3)/4;
const uint8_t *buf = (uint8_t *)pkt.data.data;
uint32_t buf32[len_words] {};
memcpy((uint8_t*)buf32, buf, len);
const uint32_t sector_size = flash_func_sector_size(fw_update.sector);
if (fw_update.sector_ofs == 0) {
flash_func_erase_sector(fw_update.sector);
}
if (fw_update.sector_ofs+len > sector_size) {
flash_func_erase_sector(fw_update.sector+1);
}
flash_write_buffer(fw_update.ofs, buf32, len_words);
fw_update.ofs += len;
fw_update.sector_ofs += len;
if (fw_update.sector_ofs >= flash_func_sector_size(fw_update.sector)) {
fw_update.sector++;
fw_update.sector_ofs -= sector_size;
}
if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) {
fw_update.node_id = 0;
flash_write_flush();
const auto ok = check_good_firmware();
node_status.vendor_specific_status_code = uint8_t(ok);
if (ok == check_fw_result_t::CHECK_FW_OK) {
jump_to_app();
/*
match the response to a sent request
*/
uint8_t idx = 0;
bool found = false;
for (idx=0; idx<FW_UPDATE_PIPELINE_LEN; idx++) {
const auto &r = fw_update.reads[idx];
if (r.tx_id == transfer->transfer_id) {
found = true;
break;
}
}
if (!found) {
// not a current transfer
return;
}
if (uavcan_protocol_file_ReadResponse_decode(transfer, &fw_update.reads[idx].pkt)) {
return;
}
fw_update.reads[idx].have_reply = true;
uint32_t rtt = MIN(3000,MAX(AP_HAL::millis() - fw_update.reads[idx].sent_ms, 25));
fw_update.rtt_ms = uint32_t(0.9 * fw_update.rtt_ms + 0.1 * rtt);
while (fw_update.reads[fw_update.idx].have_reply) {
auto &r = fw_update.reads[fw_update.idx];
if (r.offset != fw_update.ofs) {
// bad sequence
r.have_reply = false;
r.sent_ms = 0;
break;
}
const auto &pkt = r.pkt;
const uint16_t len = pkt.data.len;
const uint16_t len_words = (len+3U)/4U;
const uint8_t *buf = (uint8_t *)pkt.data.data;
uint32_t buf32[len_words] {};
memcpy((uint8_t*)buf32, buf, len);
if (fw_update.ofs == 0) {
flash_set_keep_unlocked(true);
}
const uint32_t sector_size = flash_func_sector_size(fw_update.sector);
if (sector_size == 0) {
// firmware is too big
fw_update.node_id = 0;
flash_write_flush();
flash_set_keep_unlocked(false);
node_status.vendor_specific_status_code = uint8_t(check_fw_result_t::FAIL_REASON_BAD_LENGTH_APP);
break;
}
if (fw_update.sector_ofs == 0) {
erase_to(fw_update.sector);
}
if (fw_update.sector_ofs+len > sector_size) {
erase_to(fw_update.sector+1);
}
if (!flash_write_buffer(fw_update.ofs, buf32, len_words)) {
continue;
}
fw_update.ofs += len;
fw_update.sector_ofs += len;
if (fw_update.sector_ofs >= flash_func_sector_size(fw_update.sector)) {
fw_update.sector++;
fw_update.sector_ofs -= sector_size;
}
if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) {
fw_update.node_id = 0;
flash_write_flush();
flash_set_keep_unlocked(false);
const auto ok = check_good_firmware();
node_status.vendor_specific_status_code = uint8_t(ok);
if (ok == check_fw_result_t::CHECK_FW_OK) {
jump_to_app();
}
return;
}
r.have_reply = false;
r.sent_ms = 0;
r.offset += FW_UPDATE_PIPELINE_LEN*sizeof(uavcan_protocol_file_ReadResponse::data.data);
send_fw_read(fw_update.idx);
processTx();
fw_update.idx = (fw_update.idx + 1) % FW_UPDATE_PIPELINE_LEN;
}
// show offset number we are flashing in kbyte as crude progress indicator
node_status.vendor_specific_status_code = 1 + (fw_update.ofs / 1024U);
fw_update.last_ms = 0;
}
/*
@ -243,14 +361,13 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
if (pkt.image_file_remote_path.path.len > sizeof(fw_update.path)-1) {
return;
}
memset(&fw_update, 0, sizeof(fw_update));
for (uint8_t i=0; i<FW_UPDATE_PIPELINE_LEN; i++) {
fw_update.reads[i].offset = i*sizeof(uavcan_protocol_file_ReadResponse::data.data);
}
memcpy(fw_update.path, pkt.image_file_remote_path.path.data, pkt.image_file_remote_path.path.len);
fw_update.path[pkt.image_file_remote_path.path.len] = 0;
fw_update.node_id = pkt.source_node_id;
fw_update.ofs = 0;
fw_update.last_ms = 0;
fw_update.sector = 0;
fw_update.sector_ofs = 0;
if (fw_update.node_id == 0) {
fw_update.node_id = transfer->source_node_id;
}
@ -270,8 +387,6 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
CanardResponse,
&buffer[0],
total_size);
send_fw_read();
}
static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer)
@ -606,6 +721,9 @@ bool can_check_update(void)
if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) {
can_set_node_id(comms->my_node_id);
fw_update.node_id = comms->server_node_id;
for (uint8_t i=0; i<FW_UPDATE_PIPELINE_LEN; i++) {
fw_update.reads[i].offset = i*sizeof(uavcan_protocol_file_ReadResponse::data.data);
}
memcpy(fw_update.path, comms->path, sizeof(uavcan_protocol_file_Path::path.data)+1);
ret = true;
// clear comms region
@ -693,8 +811,8 @@ void can_start()
void can_update()
{
// do one loop of CAN support. If we are doing a few update then
// loop until it is finished
// do one loop of CAN support. If we are doing a firmware update
// then loop until it is finished
do {
processTx();
processRx();
@ -706,8 +824,12 @@ void can_update()
process1HzTasks(AP_HAL::micros64());
}
if (fw_update.node_id != 0) {
send_fw_read();
send_fw_reads();
}
#if CH_CFG_ST_FREQUENCY >= 1000000
// give a bit of time for background processing
chThdSleepMicroseconds(200);
#endif
} while (fw_update.node_id != 0);
}

24
Tools/AP_Bootloader/support.cpp
View File

@ -143,6 +143,11 @@ uint32_t flash_func_sector_size(uint32_t sector)
return stm32_flash_getpagesize(flash_base_page+sector);
}
bool flash_func_is_erased(uint32_t sector)
{
return stm32_flash_ispageerased(flash_base_page+sector);
}
bool flash_func_erase_sector(uint32_t sector, bool force_erase)
{
#if AP_BOOTLOADER_ALWAYS_ERASE
@ -348,6 +353,17 @@ void uprintf(const char *fmt, ...)
#endif
}
static void thread_sleep_ms(uint32_t ms)
{
while (ms > 0) {
// don't sleep more than 65 at a time, to cope with 16 bit
// timer
const uint32_t dt = ms > 65? 65: ms;
chThdSleepMilliseconds(dt);
ms -= dt;
}
}
// generate a pulse sequence forever, for debugging
void led_pulses(uint8_t npulses)
{
@ -355,11 +371,11 @@ void led_pulses(uint8_t npulses)
while (true) {
for (uint8_t i=0; i<npulses; i++) {
led_on(LED_BOOTLOADER);
chThdSleepMilliseconds(200);
thread_sleep_ms(200);
led_off(LED_BOOTLOADER);
chThdSleepMilliseconds(200);
thread_sleep_ms(200);
}
chThdSleepMilliseconds(2000);
thread_sleep_ms(2000);
}
}
@ -431,7 +447,7 @@ void init_uarts(void)
sduStart(&SDU1, &serusbcfg1);
usbDisconnectBus(serusbcfg1.usbp);
chThdSleepMilliseconds(1000);
thread_sleep_ms(1000);
usbStart(serusbcfg1.usbp, &usbcfg);
usbConnectBus(serusbcfg1.usbp);
#endif

1
Tools/AP_Bootloader/support.h
View File

@ -25,6 +25,7 @@ uint32_t flash_func_read_word(uint32_t offset);
bool flash_func_write_word(uint32_t offset, uint32_t v);
bool flash_func_write_words(uint32_t offset, uint32_t *v, uint8_t n);
uint32_t flash_func_sector_size(uint32_t sector);
bool flash_func_is_erased(uint32_t sector);
bool flash_func_erase_sector(uint32_t sector, bool force_erase = false);
uint32_t flash_func_read_otp(uint32_t idx);
uint32_t flash_func_read_sn(uint32_t idx);