AP_Bootloader: show IP as CAN debug msg in bootloader

this also saves a few bytes of flash for normal periph bootloader
This commit is contained in:
Andrew Tridgell 2024-01-17 09:12:42 +11:00
parent b48a01dbc8
commit 1bee4630cc
3 changed files with 70 additions and 15 deletions

View File

@ -75,6 +75,10 @@ static void processTx(void);
#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;
@ -625,6 +629,29 @@ static void processRx(void)
}
#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
*/
@ -663,13 +690,12 @@ static void can_handle_DNA(void)
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));
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;
@ -684,13 +710,12 @@ static void send_node_status(void)
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);
canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
UAVCAN_PROTOCOL_NODESTATUS_ID,
transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
buffer,
len);
}
@ -832,4 +857,30 @@ void can_update()
} 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

View File

@ -2,3 +2,4 @@ void can_start();
void can_update();
void can_set_node_id(uint8_t node_id);
bool can_check_update(void);
void can_printf(const char *fmt, ...);

View File

@ -25,6 +25,7 @@
#include "bl_protocol.h"
#include <AP_CheckFirmware/AP_CheckFirmware.h>
#include "app_comms.h"
#include "can.h"
#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR
#define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46"
@ -57,10 +58,12 @@
void BL_Network::link_up_cb(void *p)
{
auto *driver = (BL_Network *)p;
#if AP_BOOTLOADER_NETWORK_USE_DHCP
auto *driver = (AP_Networking_ChibiOS *)p;
dhcp_start(driver->thisif);
#endif
char ipstr[IP4_STR_LEN];
can_printf("IP %s", SocketAPM::inet_addr_to_str(ntohl(driver->thisif->ip_addr.addr), ipstr, sizeof(ipstr)));
}
void BL_Network::link_down_cb(void *p)