mirror of https://github.com/ArduPilot/ardupilot
888 lines
29 KiB
C++
888 lines
29 KiB
C++
/*
|
|
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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
CAN bootloader support
|
|
*/
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <hal.h>
|
|
#if HAL_USE_CAN == TRUE || HAL_NUM_CAN_IFACES
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Math/crc.h>
|
|
#include <canard.h>
|
|
#include "support.h"
|
|
#include <dronecan_msgs.h>
|
|
#include "can.h"
|
|
#include "bl_protocol.h"
|
|
#include <drivers/stm32/canard_stm32.h>
|
|
#include "app_comms.h"
|
|
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
|
|
#include <stdio.h>
|
|
#include <AP_HAL_ChibiOS/CANIface.h>
|
|
#include <AP_CheckFirmware/AP_CheckFirmware.h>
|
|
|
|
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;
|
|
|
|
#if HAL_USE_CAN
|
|
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 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." CHIBIOS_BOARD_NAME
|
|
#endif
|
|
|
|
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 void processTx(void);
|
|
|
|
// keep up to 4 transfers in progress
|
|
#ifndef FW_UPDATE_PIPELINE_LEN
|
|
#define FW_UPDATE_PIPELINE_LEN 4
|
|
#endif
|
|
|
|
#if CH_CFG_USE_MUTEXES == TRUE
|
|
static HAL_Semaphore can_mutex;
|
|
#endif
|
|
|
|
static struct {
|
|
uint32_t rtt_ms;
|
|
uint32_t ofs;
|
|
uint8_t node_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;
|
|
|
|
/*
|
|
get cpu unique ID
|
|
*/
|
|
static void readUniqueID(uint8_t* out_uid)
|
|
{
|
|
uint8_t len = sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data);
|
|
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 integer in a given range
|
|
*/
|
|
static uint32_t get_random_range(uint16_t range)
|
|
{
|
|
return get_randomu16() % range;
|
|
}
|
|
|
|
/*
|
|
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);
|
|
|
|
// use hw major/minor for APJ_BOARD_ID so we know what fw is
|
|
// compatible with this hardware
|
|
pkt.hardware_version.major = APJ_BOARD_ID >> 8;
|
|
pkt.hardware_version.minor = APJ_BOARD_ID & 0xFF;
|
|
|
|
char name[strlen(CAN_APP_NODE_NAME)+1];
|
|
strcpy(name, CAN_APP_NODE_NAME);
|
|
pkt.name.len = strlen(CAN_APP_NODE_NAME);
|
|
memcpy(pkt.name.data, name, pkt.name.len);
|
|
|
|
uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, true);
|
|
|
|
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 bool send_fw_read(uint8_t idx)
|
|
{
|
|
auto &r = fw_update.reads[idx];
|
|
r.tx_id = fw_update.transfer_id;
|
|
r.have_reply = false;
|
|
|
|
uavcan_protocol_file_ReadRequest pkt {};
|
|
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);
|
|
|
|
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++;
|
|
}
|
|
}
|
|
|
|
/*
|
|
handle response to file read for fw update
|
|
*/
|
|
static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer)
|
|
{
|
|
if (transfer->source_node_id != fw_update.node_id) {
|
|
return;
|
|
}
|
|
/*
|
|
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);
|
|
}
|
|
|
|
/*
|
|
handle a begin firmware update request. We start pulling in the file data
|
|
*/
|
|
static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer)
|
|
{
|
|
if (fw_update.node_id == 0) {
|
|
uavcan_protocol_file_BeginFirmwareUpdateRequest pkt;
|
|
if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &pkt)) {
|
|
return;
|
|
}
|
|
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;
|
|
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, true);
|
|
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);
|
|
}
|
|
|
|
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 +
|
|
get_random_range(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;
|
|
}
|
|
|
|
struct uavcan_protocol_dynamic_node_id_Allocation msg;
|
|
if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) {
|
|
return;
|
|
}
|
|
|
|
// Obtaining the local unique ID
|
|
uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)];
|
|
readUniqueID(my_unique_id);
|
|
|
|
// Matching the received UID against the local one
|
|
if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) {
|
|
node_id_allocation_unique_id_offset = 0;
|
|
return; // No match, return
|
|
}
|
|
|
|
if (msg.unique_id.len < sizeof(msg.unique_id.data)) {
|
|
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
|
|
node_id_allocation_unique_id_offset = msg.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
|
|
canardSetLocalNodeID(ins, msg.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;
|
|
}
|
|
|
|
#if HAL_USE_CAN
|
|
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 {};
|
|
|
|
#ifdef HAL_GPIO_PIN_LED_BOOTLOADER
|
|
palToggleLine(HAL_GPIO_PIN_LED_BOOTLOADER);
|
|
#endif
|
|
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);
|
|
}
|
|
}
|
|
#else
|
|
// Use HAL CAN interface
|
|
static void processTx(void)
|
|
{
|
|
static uint8_t fail_count;
|
|
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
|
|
AP_HAL::CANFrame txmsg {};
|
|
txmsg.dlc = txf->data_len;
|
|
memcpy(txmsg.data, txf->data, 8);
|
|
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
|
|
// push message with 1s timeout
|
|
bool send_ok = false;
|
|
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
|
|
send_ok |= (can_iface[i].send(txmsg, AP_HAL::micros64() + 1000000, 0) > 0);
|
|
}
|
|
if (send_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)
|
|
{
|
|
AP_HAL::CANFrame rxmsg;
|
|
while (true) {
|
|
bool got_pkt = false;
|
|
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
|
|
bool read_select = true;
|
|
bool write_select = false;
|
|
can_iface[i].select(read_select, write_select, nullptr, 0);
|
|
if (!read_select) {
|
|
continue;
|
|
}
|
|
#ifdef HAL_GPIO_PIN_LED_BOOTLOADER
|
|
palToggleLine(HAL_GPIO_PIN_LED_BOOTLOADER);
|
|
#endif
|
|
CanardCANFrame rx_frame {};
|
|
|
|
//palToggleLine(HAL_GPIO_PIN_LED);
|
|
uint64_t timestamp;
|
|
AP_HAL::CANIface::CanIOFlags flags;
|
|
can_iface[i].receive(rxmsg, timestamp, flags);
|
|
memcpy(rx_frame.data, rxmsg.data, 8);
|
|
rx_frame.data_len = rxmsg.dlc;
|
|
rx_frame.id = rxmsg.id;
|
|
canardHandleRxFrame(&canard, &rx_frame, timestamp);
|
|
got_pkt = true;
|
|
}
|
|
if (!got_pkt) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
#endif //#if HAL_USE_CAN
|
|
|
|
/*
|
|
wrapper around broadcast
|
|
*/
|
|
static void canard_broadcast(uint64_t data_type_signature,
|
|
uint16_t data_type_id,
|
|
uint8_t &transfer_id,
|
|
uint8_t priority,
|
|
const void* payload,
|
|
uint16_t payload_len)
|
|
{
|
|
#if CH_CFG_USE_MUTEXES == TRUE
|
|
WITH_SEMAPHORE(can_mutex);
|
|
#endif
|
|
canardBroadcast(&canard,
|
|
data_type_signature,
|
|
data_type_id,
|
|
&transfer_id,
|
|
priority,
|
|
payload,
|
|
payload_len);
|
|
}
|
|
|
|
|
|
/*
|
|
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 +
|
|
get_random_range(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[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)];
|
|
readUniqueID(my_unique_id);
|
|
|
|
static const uint8_t MaxLenOfUniqueIDInRequest = 6;
|
|
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) {
|
|
uid_size = MaxLenOfUniqueIDInRequest;
|
|
}
|
|
|
|
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size);
|
|
|
|
// Broadcasting the request
|
|
canard_broadcast(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, true);
|
|
|
|
static uint8_t transfer_id; // Note that the transfer ID variable MUST BE STATIC (or heap-allocated)!
|
|
|
|
canard_broadcast(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_MAINTENANCE;
|
|
send_node_status();
|
|
}
|
|
}
|
|
|
|
void can_set_node_id(uint8_t node_id)
|
|
{
|
|
initial_node_id = node_id;
|
|
}
|
|
|
|
// check for a firmware update marker left by app
|
|
bool can_check_update(void)
|
|
{
|
|
bool ret = false;
|
|
#if HAL_RAM_RESERVE_START >= 256
|
|
struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START;
|
|
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
|
|
memset(comms, 0, sizeof(struct app_bootloader_comms));
|
|
}
|
|
#endif
|
|
#if defined(CAN1_BASE) && defined(RCC_APB1ENR_CAN1EN)
|
|
// check for px4 fw update. px4 uses the filter registers in CAN1
|
|
// to communicate with the bootloader. This only works on CAN1
|
|
if (!ret && stm32_was_software_reset()) {
|
|
uint32_t *fir = (uint32_t *)(CAN1_BASE + 0x240);
|
|
struct PACKED app_shared {
|
|
union {
|
|
uint64_t ull;
|
|
uint32_t ul[2];
|
|
uint8_t valid;
|
|
} crc;
|
|
uint32_t signature;
|
|
uint32_t bus_speed;
|
|
uint32_t node_id;
|
|
} *app = (struct app_shared *)&fir[4];
|
|
/* we need to enable the CAN peripheral in order to look at
|
|
the FIR registers.
|
|
*/
|
|
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
|
|
static const uint32_t app_signature = 0xb0a04150;
|
|
if (app->signature == app_signature &&
|
|
app->node_id > 0 && app->node_id < 128) {
|
|
// crc is in reversed word order in FIR registers
|
|
uint32_t sig[3];
|
|
memcpy((uint8_t *)&sig[0], (const uint8_t *)&app->signature, sizeof(sig));
|
|
const uint64_t crc = crc_crc64(sig, 3);
|
|
const uint32_t *crc32 = (const uint32_t *)&crc;
|
|
if (crc32[0] == app->crc.ul[1] &&
|
|
crc32[1] == app->crc.ul[0]) {
|
|
// reset signature so we don't get in a boot loop
|
|
app->signature = 0;
|
|
// setup node ID
|
|
can_set_node_id(app->node_id);
|
|
// and baudrate
|
|
baudrate = app->bus_speed;
|
|
ret = true;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
return ret;
|
|
}
|
|
|
|
void can_start()
|
|
{
|
|
node_status.vendor_specific_status_code = uint8_t(check_good_firmware());
|
|
node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE;
|
|
|
|
#if HAL_USE_CAN
|
|
// 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);
|
|
#else
|
|
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
|
|
can_iface[i].init(baudrate, AP_HAL::CANIface::NormalMode);
|
|
}
|
|
#endif
|
|
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 +
|
|
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
|
|
|
|
if (stm32_was_watchdog_reset()) {
|
|
node_status.vendor_specific_status_code = uint8_t(check_fw_result_t::FAIL_REASON_WATCHDOG);
|
|
}
|
|
}
|
|
|
|
|
|
void can_update()
|
|
{
|
|
// do one loop of CAN support. If we are doing a firmware 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_reads();
|
|
}
|
|
#if CH_CFG_ST_FREQUENCY >= 1000000
|
|
// give a bit of time for background processing
|
|
chThdSleepMicroseconds(200);
|
|
#endif
|
|
} while (fw_update.node_id != 0);
|
|
}
|
|
|
|
// printf to CAN LogMessage for debugging
|
|
void can_printf(const char *fmt, ...)
|
|
{
|
|
// only on H7 for now, where we have plenty of flash
|
|
#if defined(STM32H7)
|
|
uavcan_protocol_debug_LogMessage pkt {};
|
|
uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
|
|
va_list ap;
|
|
va_start(ap, fmt);
|
|
uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap);
|
|
va_end(ap);
|
|
pkt.text.len = MIN(n, sizeof(pkt.text.data));
|
|
|
|
uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, true);
|
|
static uint8_t logmsg_transfer_id;
|
|
|
|
canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
|
|
UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
|
|
logmsg_transfer_id,
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
|
buffer,
|
|
len);
|
|
#endif // defined(STM32H7)
|
|
}
|
|
|
|
|
|
#endif // HAL_USE_CAN
|