Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Bootloader: use dronecan_dsdlc generated code instead

Browse Source
This commit is contained in:
bugobliterator 2022-10-24 08:45:00 +05:30 committed by Andrew Tridgell
parent e4ada09fc3
commit 0b3197727a
1 changed files with 11 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Tools/AP_Bootloader/can.cpp
View File

@ -22,15 +22,7 @@
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include <canard.h> #include <canard.h>
#include "support.h" #include "support.h"
#include <uavcan/protocol/dynamic_node_id/Allocation.h> #include <dronecan_msgs.h>
#include <uavcan/protocol/file/BeginFirmwareUpdate.h>
#include <uavcan/protocol/file/Read.h>
#include <uavcan/protocol/dynamic_node_id/Allocation.h>
#include <uavcan/protocol/NodeStatus.h>
#include <uavcan/protocol/RestartNode.h>
#include <uavcan/protocol/GetNodeInfo.h>
#include <uavcan/equipment/indication/LightsCommand.h>
#include <ardupilot/indication/NotifyState.h>
#include "can.h" #include "can.h"
#include "bl_protocol.h" #include "bl_protocol.h"
#include <drivers/stm32/canard_stm32.h> #include <drivers/stm32/canard_stm32.h>
@ -79,7 +71,7 @@ static struct {
uint32_t last_ms; uint32_t last_ms;
uint8_t node_id; uint8_t node_id;
uint8_t transfer_id; uint8_t transfer_id;
uint8_t path[UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1]; uint8_t path[sizeof(uavcan_protocol_file_Path::path.data)+1];
uint8_t sector; uint8_t sector;
uint32_t sector_ofs; uint32_t sector_ofs;
} fw_update; } fw_update;
@ -89,7 +81,7 @@ static struct {
*/ */
static void readUniqueID(uint8_t* out_uid) static void readUniqueID(uint8_t* out_uid)
{ {
uint8_t len = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH; uint8_t len = sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data);
memset(out_uid, 0, len); memset(out_uid, 0, len);
memcpy(out_uid, (const void *)UDID_START, MIN(len,12)); memcpy(out_uid, (const void *)UDID_START, MIN(len,12));
} }
@ -140,7 +132,7 @@ static void handle_get_node_info(CanardInstance* ins,
char name[strlen(CAN_APP_NODE_NAME)+1]; char name[strlen(CAN_APP_NODE_NAME)+1];
strcpy(name, CAN_APP_NODE_NAME); strcpy(name, CAN_APP_NODE_NAME);
pkt.name.len = strlen(CAN_APP_NODE_NAME); pkt.name.len = strlen(CAN_APP_NODE_NAME);
pkt.name.data = (uint8_t *)name; memcpy(pkt.name.data, name, pkt.name.len);
uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, true); uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, true);
@ -225,7 +217,7 @@ static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* tra
fw_update.sector++; fw_update.sector++;
fw_update.sector_ofs -= sector_size; fw_update.sector_ofs -= sector_size;
} }
if (len < UAVCAN_PROTOCOL_FILE_READ_RESPONSE_DATA_MAX_LENGTH) { if (len < sizeof(uavcan_protocol_file_ReadResponse::data.data)) {
fw_update.node_id = 0; fw_update.node_id = 0;
flash_write_flush(); flash_write_flush();
const auto ok = check_good_firmware(); const auto ok = check_good_firmware();
@ -301,7 +293,7 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
// Copying the unique ID from the message // Copying the unique ID from the message
static const uint8_t UniqueIDBitOffset = 8; 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[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)];
uint8_t received_unique_id_len = 0; uint8_t received_unique_id_len = 0;
for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) { for (; received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); received_unique_id_len++) {
const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U); const uint8_t bit_offset = (uint8_t)(UniqueIDBitOffset + received_unique_id_len * 8U);
@ -309,7 +301,7 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
} }
// Obtaining the local unique ID // Obtaining the local unique ID
uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)];
readUniqueID(my_unique_id); readUniqueID(my_unique_id);
// Matching the received UID against the local one // Matching the received UID against the local one
@ -318,7 +310,7 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
return; // No match, return return; // No match, return
} }
if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) { if (received_unique_id_len < sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)) {
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. // 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; 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; send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
@ -555,11 +547,11 @@ static void can_handle_DNA(void)
allocation_request[0] |= 1; // First part of unique ID allocation_request[0] |= 1; // First part of unique ID
} }
uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)];
readUniqueID(my_unique_id); readUniqueID(my_unique_id);
static const uint8_t MaxLenOfUniqueIDInRequest = 6; 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); uint8_t uid_size = (uint8_t)(sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data) - node_id_allocation_unique_id_offset);
if (uid_size > MaxLenOfUniqueIDInRequest) { if (uid_size > MaxLenOfUniqueIDInRequest) {
uid_size = MaxLenOfUniqueIDInRequest; uid_size = MaxLenOfUniqueIDInRequest;
} }
@ -625,7 +617,7 @@ bool can_check_update(void)
if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) { if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) {
can_set_node_id(comms->my_node_id); can_set_node_id(comms->my_node_id);
fw_update.node_id = comms->server_node_id; fw_update.node_id = comms->server_node_id;
memcpy(fw_update.path, comms->path, UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1); memcpy(fw_update.path, comms->path, sizeof(uavcan_protocol_file_Path::path.data)+1);
ret = true; ret = true;
// clear comms region // clear comms region
memset(comms, 0, sizeof(struct app_bootloader_comms)); memset(comms, 0, sizeof(struct app_bootloader_comms));