Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

examples: added firmware update to ESCNode example #67

Merged
merged 2 commits into from
Apr 2, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions examples/ESCNode/Makefile
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,10 @@ LIBS+=dsdl_generated/src/uavcan.protocol.param.GetSet_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.param.GetSet_res.c
LIBS+=dsdl_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.BeginFirmwareUpdate_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.BeginFirmwareUpdate_res.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.Read_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.Read_res.c

all: esc_node

Expand Down
225 changes: 221 additions & 4 deletions examples/ESCNode/esc_node.c
Original file line number Diff line number Diff line change
@@ -1,14 +1,15 @@
/*
A simple example DroneCAN node implementing a 4-in-1 ESC

This example implements 6 features:
This example implements 7 features:

- announces on the bus using NodeStatus at 1Hz
- answers GetNodeInfo requests
- implements dynamic node allocation
- listens for ESC RawCommand commands and extracts throttle levels
- sends ESC Status messages (with synthetic data based on throttles)
- a parameter server for reading and writing node parameters
- firmware update

This example uses socketcan or multicast UDP on Linux for CAN transport

Expand Down Expand Up @@ -69,6 +70,18 @@ static struct esc_state {
uint64_t last_update_us;
} esc;

/*
keep the state for firmware update
*/
static struct {
char path[256];
uint8_t node_id;
uint8_t transfer_id;
uint32_t last_read_ms;
int fd;
uint32_t offset;
} fwupdate;

/*
state of user settings. This will be saved in settings.dat. On a
real device a better storage system will be needed
Expand Down Expand Up @@ -164,6 +177,8 @@ static void save_settings(void)
*/
static void load_settings(void)
{
fwupdate.fd = -1;

int fd = open("settings.dat", O_RDONLY);
if (fd == -1) {
return;
Expand Down Expand Up @@ -393,7 +408,10 @@ static void handle_DNA_Allocation(CanardInstance *ins, CanardRxTransfer *transfe
// Copying the unique ID from the message
struct uavcan_protocol_dynamic_node_id_Allocation msg;

uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg);
if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) {
/* bad packet */
return;
}

// Obtaining the local unique ID
uint8_t my_unique_id[sizeof(msg.unique_id.data)];
Expand Down Expand Up @@ -471,6 +489,170 @@ static void request_DNA()
}


/*
handle a BeginFirmwareUpdate request from a management tool like DroneCAN GUI tool or MissionPlanner

There are multiple ways to handle firmware update over DroneCAN:

1) on BeginFirmwareUpdate reboot to the bootloader, and implement
the firmware upudate process in the bootloader. This is good on
boards with smaller amounts of flash

2) if you have enough flash for 2 copies of your firmware then you
can use an A/B scheme, where the new firmware is saved to the
inactive flash region and a tag is used to indicate which
firmware to boot next time

3) you could write the firmware to secondary storage (such as a
microSD) and the bootloader would flash it on next boot

In this example firmware we will write it to a file
newfirmware.bin, which is option 3

Note that you cannot rely on the form of the filename. The client
may hash the filename before sending
*/
static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer)
{
/*
on real hardware this is where you would save the current node
ID to some piece of memory that is not cleared by reboot so the
bootloader knows what node number to use. Most MCUs have some
registers (eg. RTC/backup registers) that can be used for that
purpose.
*/

/*
decode the request
*/
struct uavcan_protocol_file_BeginFirmwareUpdateRequest req;
if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) {
return;
}

/*
check for a repeated BeginFirmwareUpdateRequest
*/
if (fwupdate.node_id == transfer->source_node_id &&
fwupdate.fd != -1 &&
memcmp(fwupdate.path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len) == 0) {
/* ignore duplicate request */
return;
}


/*
open the file to hold the new firmware
*/
if (fwupdate.fd != -1) {
close(fwupdate.fd);
}
fwupdate.fd = open("newfirmware.bin", O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (fwupdate.fd == -1) {
printf("Open of newfirmware.bin failed\n");
return;
}

fwupdate.offset = 0;
fwupdate.node_id = transfer->source_node_id;
strncpy(fwupdate.path, (char*)req.image_file_remote_path.path.data, req.image_file_remote_path.path.len);

uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE];
struct uavcan_protocol_file_BeginFirmwareUpdateResponse reply;
memset(&reply, 0, sizeof(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);

printf("Started firmware update\n");

/*
this is where you would reboot to the bootloader if implementing option (1) above
*/
}

/*
send a read for a firmware update. This asks the client (firmware
server) for a piece of the new firmware
*/
static void send_firmware_read(void)
{
uint32_t now = millis32();
if (now - fwupdate.last_read_ms < 750) {
// the server may still be responding
return;
}
fwupdate.last_read_ms = now;

uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE];

struct uavcan_protocol_file_ReadRequest pkt;
memset(&pkt, 0, sizeof(pkt));

pkt.path.path.len = strlen((const char *)fwupdate.path);
pkt.offset = fwupdate.offset;
memcpy(pkt.path.path.data, fwupdate.path, pkt.path.path.len);

uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer);

canardRequestOrRespond(&canard,
fwupdate.node_id,
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
UAVCAN_PROTOCOL_FILE_READ_ID,
&fwupdate.transfer_id,
CANARD_TRANSFER_PRIORITY_HIGH,
CanardRequest,
&buffer[0],
total_size);
}

/*
handle response to send_firmware_read()
*/
static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer)
{
if ((transfer->transfer_id+1)%32 != fwupdate.transfer_id ||
transfer->source_node_id != fwupdate.node_id) {
/* not for us */
printf("Firmware update: not for us id=%u/%u\n", (unsigned)transfer->transfer_id, (unsigned)fwupdate.transfer_id);
return;
}
struct uavcan_protocol_file_ReadResponse pkt;
if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) {
/* bad packet */
printf("Firmware update: bad packet\n");
return;
}
if (pkt.error.value != UAVCAN_PROTOCOL_FILE_ERROR_OK) {
/* read failed */
fwupdate.node_id = 0;
printf("Firmware update read failure\n");
return;
}
write(fwupdate.fd, pkt.data.data, pkt.data.len);
if (pkt.data.len < 256) {
/* firmware updare done */
close(fwupdate.fd);
printf("Firmwate update complete\n");
fwupdate.node_id = 0;
return;
}
fwupdate.offset += pkt.data.len;

/* trigger a new read */
fwupdate.last_read_ms = 0;
}

/*
This callback is invoked by the library when a new message or request or response is received.
*/
Expand All @@ -496,6 +678,16 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
handle_RestartNode(ins, transfer);
break;
}
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
handle_begin_firmware_update(ins, transfer);
break;
}
}
if (transfer->transfer_type == CanardTransferTypeResponse) {
switch (transfer->data_type_id) {
case UAVCAN_PROTOCOL_FILE_READ_ID:
handle_file_read_response(ins, transfer);
break;
}
}
if (transfer->transfer_type == CanardTransferTypeBroadcast) {
Expand Down Expand Up @@ -548,7 +740,18 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
*out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
return true;
}
}
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE;
return true;
}
}
if (transfer_type == CanardTransferTypeResponse) {
// check if we want to handle a specific service request
switch (data_type_id) {
case UAVCAN_PROTOCOL_FILE_READ_ID:
*out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE;
return true;
}
}
if (transfer_type == CanardTransferTypeBroadcast) {
// see if we want to handle a specific broadcast packet
Expand Down Expand Up @@ -579,9 +782,19 @@ static void send_NodeStatus(void)
node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
node_status.sub_mode = 0;

// put whatever you like in here for display in GUI
node_status.vendor_specific_status_code = 1234;

/*
when doing a firmware update put the size in kbytes in VSSC so
the user can see how far it has reached
*/
if (fwupdate.node_id != 0) {
node_status.vendor_specific_status_code = fwupdate.offset / 1024;
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE;
}

uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);

// we need a static variable for the transfer ID. This is
Expand Down Expand Up @@ -756,7 +969,11 @@ int main(int argc, char** argv)
if (ts >= next_50hz_service_at) {
next_50hz_service_at += 1000000ULL/50U;
send_ESCStatus();
}
}

if (fwupdate.node_id != 0) {
send_firmware_read();
}
}

return 0;
Expand Down
8 changes: 7 additions & 1 deletion examples/ServoNode/Makefile
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,14 @@
CANARD_BASE=../..

CC=gcc
CFLAGS = -g -Wall -Idsdl_generated/include -I$(CANARD_BASE) -I$(CANARD_BASE)/drivers/socketcan
CFLAGS = -g -Wall -Idsdl_generated/include -I$(CANARD_BASE) -I$(CANARD_BASE)/drivers/linux

LIBS=$(CANARD_BASE)/canard.c

# add socketcan driver for linux
LIBS+=$(CANARD_BASE)/drivers/socketcan/socketcan.c
LIBS+=$(CANARD_BASE)/drivers/mcast/mcast.c
LIBS+=$(CANARD_BASE)/drivers/linux/linux.c

# add in generated code
LIBS+=dsdl_generated/src/uavcan.protocol.NodeStatus.c
Expand All @@ -19,6 +21,10 @@ LIBS+=dsdl_generated/src/uavcan.protocol.param.GetSet_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.param.GetSet_res.c
LIBS+=dsdl_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.BeginFirmwareUpdate_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.BeginFirmwareUpdate_res.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.Read_req.c
LIBS+=dsdl_generated/src/uavcan.protocol.file.Read_res.c

all: servo_node

Expand Down
Loading
Loading