From 3bc5458a826a97af8ec7ce36845076285a469744 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 27 May 2019 11:38:46 +1000 Subject: [PATCH] AP_Bootloader: added CAN support for AP_Periph --- Tools/AP_Bootloader/AP_Bootloader.cpp | 37 +- Tools/AP_Bootloader/bl_protocol.cpp | 7 +- Tools/AP_Bootloader/can.cpp | 587 ++++++++++++++++++++++++++ Tools/AP_Bootloader/can.h | 3 + Tools/AP_Bootloader/mcu_f1.h | 17 + Tools/AP_Bootloader/support.cpp | 17 +- Tools/AP_Bootloader/wscript | 26 +- 7 files changed, 681 insertions(+), 13 deletions(-) create mode 100644 Tools/AP_Bootloader/can.cpp create mode 100644 Tools/AP_Bootloader/can.h create mode 100644 Tools/AP_Bootloader/mcu_f1.h diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index 966d0f09f1..5fcf454ba1 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -30,6 +30,7 @@ #include #include "support.h" #include "bl_protocol.h" +#include "can.h" extern "C" { int main(void); @@ -53,6 +54,7 @@ int main(void) bool try_boot = false; uint32_t timeout = HAL_BOOTLOADER_TIMEOUT; +#ifndef NO_FASTBOOT enum rtc_boot_magic m = check_fast_reboot(); if (stm32_was_watchdog_reset()) { try_boot = true; @@ -63,22 +65,55 @@ int main(void) try_boot = true; timeout = 0; } +#if HAL_USE_CAN == TRUE + else if ((m & 0xFFFFFF00) == RTC_BOOT_CANBL) { + try_boot = false; + timeout = 10000; + can_set_node_id(m & 0xFF); + } +#endif // if we fail to boot properly we want to pause in bootloader to give // a chance to load new app code set_fast_reboot(RTC_BOOT_OFF); +#endif + +#ifdef HAL_GPIO_PIN_STAY_IN_BOOTLOADER + // optional "stay in bootloader" pin + if (palReadLine(HAL_GPIO_PIN_STAY_IN_BOOTLOADER) == 0) { + try_boot = false; + timeout = 10000; + } +#endif if (try_boot) { jump_to_app(); } +#if defined(BOOTLOADER_DEV_LIST) init_uarts(); +#endif +#if HAL_USE_CAN == TRUE + can_start(); +#endif flash_init(); - + +#if defined(BOOTLOADER_DEV_LIST) while (true) { bootloader(timeout); jump_to_app(); } +#else + // CAN only + while (true) { + uint32_t t0 = AP_HAL::millis(); + while (AP_HAL::millis() - t0 <= timeout) { + can_update(); + chThdSleep(chTimeMS2I(1)); + } + jump_to_app(); + } +#endif } diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index f9c03a7e45..d2b57cc1d4 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -47,6 +47,7 @@ #include "bl_protocol.h" #include "support.h" +#include "can.h" // #pragma GCC optimize("O0") @@ -498,7 +499,11 @@ bootloader(unsigned timeout) /* try to get a byte from the host */ c = cin(0); - +#if HAL_USE_CAN == TRUE + if (c < 0) { + can_update(); + } +#endif } while (c < 0); led_on(LED_ACTIVITY); diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp new file mode 100644 index 0000000000..49f015aec8 --- /dev/null +++ b/Tools/AP_Bootloader/can.cpp @@ -0,0 +1,587 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + CAN bootloader support + */ +#include + +#if HAL_USE_CAN == TRUE +#include +#include +#include "support.h" +#include +#include +#include +#include +#include +#include +#include +#include "can.h" +#include "bl_protocol.h" +#include + + +static CanardInstance canard; +static uint32_t canard_memory_pool[4096/4]; +#ifndef HAL_CAN_DEFAULT_NODE_ID +#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID +#endif +static uint8_t initial_node_id = HAL_CAN_DEFAULT_NODE_ID; + +// can config for 1MBit +static uint32_t baudrate = 1000000U; + +static CANConfig cancfg = { + CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, + 0 // filled in below +}; + +#ifndef CAN_APP_VERSION_MAJOR +#define CAN_APP_VERSION_MAJOR 1 +#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" +#endif + +// darn, libcanard generates the wrong signature for file read +//#undef UAVCAN_PROTOCOL_FILE_READ_SIGNATURE +//#define UAVCAN_PROTOCOL_FILE_READ_SIGNATURE 0x8DCDCA939F33F678ULL + +static uint8_t node_id_allocation_transfer_id; +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 uint32_t app_first_word = 0xFFFFFFFF; + +static struct { + uint64_t ofs; + uint32_t last_ms; + uint8_t node_id; + uint8_t transfer_id; + uint8_t path[UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1]; + uint8_t sector; + uint32_t sector_ofs; +} fw_update; + + +/* + get cpu unique ID + */ +static void readUniqueID(uint8_t* out_uid) +{ + uint8_t len = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH; + memset(out_uid, 0, len); + memcpy(out_uid, (const void *)UDID_START, MIN(len,12)); +} + +/* + simple 16 bit random number generator + */ +static uint16_t get_randomu16(void) +{ + static uint32_t m_z = 1234; + static uint32_t m_w = 76542; + m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16); + m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16); + return ((m_z << 16) + m_w) & 0xFFFF; +} + + +/** + * Returns a pseudo random float in the range [0, 1]. + */ +static float getRandomFloat(void) +{ + return float(get_randomu16()) / 0xFFFF; +} + +/* + handle a GET_NODE_INFO request + */ +static void handle_get_node_info(CanardInstance* ins, + CanardRxTransfer* transfer) +{ + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {}; + uavcan_protocol_GetNodeInfoResponse pkt {}; + + node_status.uptime_sec = AP_HAL::millis() / 1000U; + + pkt.status = node_status; + pkt.software_version.major = CAN_APP_VERSION_MAJOR; + pkt.software_version.minor = CAN_APP_VERSION_MINOR; + + readUniqueID(pkt.hardware_version.unique_id); + + char name[strlen(CAN_APP_NODE_NAME)+1]; + strcpy(name, CAN_APP_NODE_NAME); + pkt.name.len = strlen(CAN_APP_NODE_NAME); + pkt.name.data = (uint8_t *)name; + + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); +} + +/* + send a read for a fw update + */ +static void send_fw_read(void) +{ + uint32_t now = AP_HAL::millis(); + if (now - fw_update.last_ms < 500) { + return; + } + fw_update.last_ms = now; + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; + canardEncodeScalar(buffer, 0, 40, &fw_update.ofs); + uint32_t offset = 40; + uint8_t len = strlen((const char *)fw_update.path); + for (uint8_t i=0; itransfer_id+1)%256 != fw_update.transfer_id || + transfer->source_node_id != fw_update.node_id) { + return; + } + int16_t error = 0; + canardDecodeScalar(transfer, 0, 16, true, (void*)&error); + uint16_t len = transfer->payload_len - 2; + + uint32_t offset = 16; + uint32_t buf32[(len+3)/4]; + uint8_t *buf = (uint8_t *)&buf32[0]; + for (uint16_t i=0; i sector_size) { + flash_func_erase_sector(fw_update.sector+1); + } + for (uint16_t i=0; i= flash_func_sector_size(fw_update.sector)) { + fw_update.sector++; + fw_update.sector_ofs -= sector_size; + } + if (len < UAVCAN_PROTOCOL_FILE_READ_RESPONSE_DATA_MAX_LENGTH) { + fw_update.node_id = 0; + // now flash the first word + flash_func_write_word(0, app_first_word); + jump_to_app(); + } + + // 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; +} + +/* + handle a begin firmware update request. We start pulling in the file data + */ +static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) +{ + // manual decoding due to TAO bug in libcanard generated code + if (transfer->payload_len < 1 || transfer->payload_len > sizeof(fw_update.path)+1) { + return; + } + uint32_t offset = 0; + canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id); + offset += 8; + for (uint8_t i=0; ipayload_len-1; i++) { + canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]); + offset += 8; + } + 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; + } + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; + uavcan_protocol_file_BeginFirmwareUpdateResponse reply {}; + reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; + + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); + + send_fw_read(); +} + +static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) +{ + // Rule C - updating the randomized time interval + send_next_node_id_allocation_request_at_ms = + AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (uint32_t)(getRandomFloat() * UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + + if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) + { + node_id_allocation_unique_id_offset = 0; + return; + } + + // Copying the unique ID from the message + static const uint8_t UniqueIDBitOffset = 8; + uint8_t received_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; + uint8_t received_unique_id_len = 0; + for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) { + assert(received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH); + const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U); + (void) canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); + } + + // Obtaining the local unique ID + uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; + readUniqueID(my_unique_id); + + // Matching the received UID against the local one + if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { + node_id_allocation_unique_id_offset = 0; + return; // No match, return + } + + if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) { + // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. + node_id_allocation_unique_id_offset = received_unique_id_len; + send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + } else { + // Allocation complete - copying the allocated node ID from the message + uint8_t allocated_node_id = 0; + (void) canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); + assert(allocated_node_id <= 127); + + canardSetLocalNodeID(ins, allocated_node_id); + } +} + +/** + * This callback is invoked by the library when a new message or request or response is received. + */ +static void onTransferReceived(CanardInstance* ins, + CanardRxTransfer* transfer) +{ + /* + * Dynamic node ID allocation protocol. + * Taking this branch only if we don't have a node ID, ignoring otherwise. + */ + if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { + if (transfer->transfer_type == CanardTransferTypeBroadcast && + transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { + handle_allocation_response(ins, transfer); + } + return; + } + + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + handle_get_node_info(ins, transfer); + break; + + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: + handle_begin_firmware_update(ins, transfer); + break; + + case UAVCAN_PROTOCOL_FILE_READ_ID: + handle_file_read_response(ins, transfer); + break; + + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + NVIC_SystemReset(); + break; + } +} + + +/** + * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received + * by the local node. + * If the callback returns true, the library will receive the transfer. + * If the callback returns false, the library will ignore the transfer. + * All transfers that are addressed to other nodes are always ignored. + */ +static bool shouldAcceptTransfer(const CanardInstance* ins, + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) +{ + (void)source_node_id; + + if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { + /* + * If we're in the process of allocation of dynamic node ID, accept only relevant transfers. + */ + if ((transfer_type == CanardTransferTypeBroadcast) && + (data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID)) + { + *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; + return true; + } + return false; + } + + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_FILE_READ_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; + return true; + default: + break; + } + + return false; +} + +static void processTx(void) +{ + static uint8_t fail_count; + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { + CANTxFrame txmsg {}; + txmsg.DLC = txf->data_len; + memcpy(txmsg.data8, txf->data, 8); + txmsg.EID = txf->id & CANARD_CAN_EXT_ID_MASK; + txmsg.IDE = 1; + txmsg.RTR = 0; + if (canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE) == MSG_OK) { + canardPopTxQueue(&canard); + fail_count = 0; + } else { + // just exit and try again later. If we fail 8 times in a row + // then start discarding to prevent the pool filling up + if (fail_count < 8) { + fail_count++; + } else { + canardPopTxQueue(&canard); + } + return; + } + } +} + +static void processRx(void) +{ + CANRxFrame rxmsg {}; + while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) { + CanardCANFrame rx_frame {}; + + palToggleLine(HAL_GPIO_PIN_LED_BOOTLOADER); + + const uint64_t timestamp = AP_HAL::micros64(); + memcpy(rx_frame.data, rxmsg.data8, 8); + rx_frame.data_len = rxmsg.DLC; + if(rxmsg.IDE) { + rx_frame.id = CANARD_CAN_FRAME_EFF | rxmsg.EID; + } else { + rx_frame.id = rxmsg.SID; + } + canardHandleRxFrame(&canard, &rx_frame, timestamp); + } +} + +/* + handle waiting for a node ID + */ +static void can_handle_DNA(void) +{ + if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { + return; + } + + if (AP_HAL::millis() < send_next_node_id_allocation_request_at_ms) { + return; + } + + send_next_node_id_allocation_request_at_ms = + AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (uint32_t)(getRandomFloat() * UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + + // Structure of the request is documented in the DSDL definition + // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation + uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; + allocation_request[0] = (uint8_t)(CANARD_BROADCAST_NODE_ID << 1U); + + if (node_id_allocation_unique_id_offset == 0) { + allocation_request[0] |= 1; // First part of unique ID + } + + uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; + readUniqueID(my_unique_id); + + static const uint8_t MaxLenOfUniqueIDInRequest = 6; + uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - node_id_allocation_unique_id_offset); + if (uid_size > MaxLenOfUniqueIDInRequest) { + uid_size = MaxLenOfUniqueIDInRequest; + } + + memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); + + // Broadcasting the request + canardBroadcast(&canard, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, + &node_id_allocation_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + &allocation_request[0], + (uint16_t) (uid_size + 1)); + + // Preparing for timeout; if response is received, this value will be updated from the callback. + node_id_allocation_unique_id_offset = 0; +} + +static void send_node_status(void) +{ + uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; + node_status.uptime_sec = AP_HAL::millis() / 1000U; + + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + + static uint8_t transfer_id; // Note that the transfer ID variable MUST BE STATIC (or heap-allocated)! + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); +} + + +/** + * This function is called at 1 Hz rate from the main loop. + */ +static void process1HzTasks(uint64_t timestamp_usec) +{ + canardCleanupStaleTransfers(&canard, timestamp_usec); + + if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { + node_status.mode = fw_update.node_id?UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + send_node_status(); + } +} + +void can_set_node_id(uint8_t node_id) +{ + initial_node_id = node_id; +} + +void can_start() +{ + // calculate optimal CAN timings given PCLK1 and baudrate + CanardSTM32CANTimings timings {}; + canardSTM32ComputeCANTimings(STM32_PCLK1, baudrate, &timings); + cancfg.btr = CAN_BTR_SJW(0) | + CAN_BTR_TS2(timings.bit_segment_2-1) | + CAN_BTR_TS1(timings.bit_segment_1-1) | + CAN_BTR_BRP(timings.bit_rate_prescaler-1); + canStart(&CAND1, &cancfg); + + canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + + if (initial_node_id != CANARD_BROADCAST_NODE_ID) { + canardSetLocalNodeID(&canard, initial_node_id); + } + + send_next_node_id_allocation_request_at_ms = + AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (uint32_t)(getRandomFloat() * UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); +} + + +void can_update() +{ + // do one loop of CAN support. If we are doing a few update then + // loop until it is finished + do { + processTx(); + processRx(); + can_handle_DNA(); + static uint32_t last_1Hz_ms; + uint32_t now = AP_HAL::millis(); + if (now - last_1Hz_ms >= 1000) { + last_1Hz_ms = now; + process1HzTasks(AP_HAL::micros64()); + } + if (fw_update.node_id != 0) { + send_fw_read(); + } + } while (fw_update.node_id != 0); +} + +#endif // HAL_USE_CAN diff --git a/Tools/AP_Bootloader/can.h b/Tools/AP_Bootloader/can.h new file mode 100644 index 0000000000..e65b5ca869 --- /dev/null +++ b/Tools/AP_Bootloader/can.h @@ -0,0 +1,3 @@ +void can_start(); +void can_update(); +void can_set_node_id(uint8_t node_id); diff --git a/Tools/AP_Bootloader/mcu_f1.h b/Tools/AP_Bootloader/mcu_f1.h new file mode 100644 index 0000000000..4132a4098e --- /dev/null +++ b/Tools/AP_Bootloader/mcu_f1.h @@ -0,0 +1,17 @@ +/* + support tables for STM32F1 + */ + +#if defined(STM32F1) +#define STM32_UNKNOWN 0 + +const mcu_des_t mcu_descriptions[] = { + { STM32_UNKNOWN, "STM32F1xx", '?'}, +}; + +const mcu_rev_t silicon_revs[] = { +}; + +#endif // STM32F1 + + diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index fbaa8701bb..80f412e671 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -10,10 +10,15 @@ #include #include #include "support.h" +#include "mcu_f1.h" #include "mcu_f4.h" #include "mcu_f7.h" #include "mcu_h7.h" +// optional uprintf() code for debug +// #define BOOTLOADER_DEBUG SD1 + +#if defined(BOOTLOADER_DEV_LIST) static BaseChannel *uarts[] = { BOOTLOADER_DEV_LIST }; #if HAL_USE_SERIAL == TRUE static SerialConfig sercfg; @@ -25,10 +30,6 @@ static uint8_t last_uart; #define BOOTLOADER_BAUDRATE 115200 #endif -// optional uprintf() code for debug -// #define BOOTLOADER_DEBUG SD7 - - // #pragma GCC optimize("O0") int16_t cin(unsigned timeout_ms) @@ -65,6 +66,7 @@ void cout(uint8_t *data, uint32_t len) { chnWriteTimeout(uarts[last_uart], data, len, chTimeMS2I(100)); } +#endif // BOOTLOADER_DEV_LIST static uint32_t flash_base_page; static uint8_t num_pages; @@ -262,11 +264,12 @@ void uprintf(const char *fmt, ...) #ifdef BOOTLOADER_DEBUG va_list ap; static bool initialised; + static SerialConfig debug_sercfg; char umsg[200]; if (!initialised) { initialised = true; - sercfg.speed = 57600; - sdStart(&BOOTLOADER_DEBUG, &sercfg); + debug_sercfg.speed = 57600; + sdStart(&BOOTLOADER_DEBUG, &debug_sercfg); } va_start(ap, fmt); uint32_t n = vsnprintf(umsg, sizeof(umsg), fmt, ap); @@ -342,6 +345,7 @@ void *memset(void *s, int c, size_t n) return s; } +#if defined(BOOTLOADER_DEV_LIST) void lock_bl_port(void) { locked_uart = last_uart; @@ -394,3 +398,4 @@ void port_setbaud(uint32_t baudrate) sdStart((SerialDriver *)uarts[last_uart], &sercfg); #endif } +#endif // BOOTLOADER_DEV_LIST diff --git a/Tools/AP_Bootloader/wscript b/Tools/AP_Bootloader/wscript index d9219b8c1d..3b41e9a1aa 100644 --- a/Tools/AP_Bootloader/wscript +++ b/Tools/AP_Bootloader/wscript @@ -2,8 +2,24 @@ # encoding: utf-8 def build(bld): - if bld.env.BOOTLOADER: - bld.ap_program( - use='ap', - program_groups='bootloader' - ) + if not bld.env.BOOTLOADER: + return + + # build external libcanard library + bld.stlib(source='../../modules/libcanard/canard.c', + target='libcanard') + + bld.ap_program( + use=['ap','libcanard'], + program_groups='bootloader', + includes=[bld.env.SRCROOT + '/modules/libcanard', + bld.env.BUILDROOT + '/modules/libcanard/dsdlc_generated'] + ) + + bld( + # build libcanard headers + source=bld.path.ant_glob("modules/uavcan/dsdl/**/*.uavcan libraries/AP_UAVCAN/dsdl/**/*.uavcan"), + rule="python3 ../../modules/libcanard/dsdl_compiler/libcanard_dsdlc --header_only --outdir ${BUILDROOT}/modules/libcanard/dsdlc_generated ../../modules/uavcan/dsdl/uavcan", + group='dynamic_sources', + ) +