From 30195ea6b5776f63d8048a20eb5b0792177263ba Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2019 13:35:01 +1100 Subject: [PATCH] AP_Periph: added application descriptor this is used by MissionPlanner to see if correct fw is already loaded --- Tools/AP_Periph/AP_Periph.cpp | 13 +++++++++++++ Tools/AP_Periph/AP_Periph.h | 21 +++++++++++++++++++++ Tools/AP_Periph/can.cpp | 15 +++++++++------ 3 files changed, 43 insertions(+), 6 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 0a305fe9e8..84381a932f 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -47,6 +47,8 @@ void loop(void) static uint32_t start_ms; +const struct app_descriptor app_descriptor __attribute__((section(".app_descriptor")));; + void AP_Periph_FW::init() { hal.uartA->begin(AP_SERIALMANAGER_CONSOLE_BAUD, 32, 128); @@ -65,6 +67,17 @@ void AP_Periph_FW::init() AFIO->MAPR = mapr | AFIO_MAPR_CAN_REMAP_REMAP2 | AFIO_MAPR_SPI3_REMAP; #endif + printf("Booting %08x:%08x %u/%u len=%u 0x%08x\n", + app_descriptor.image_crc1, + app_descriptor.image_crc2, + app_descriptor.version_major, app_descriptor.version_minor, + app_descriptor.image_size, + app_descriptor.git_hash); + + if (hal.util->was_watchdog_reset()) { + printf("Reboot after watchdog reset\n"); + } + #ifdef HAL_PERIPH_ENABLE_GPS gps.init(serial_manager); #endif diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 4cfb486fc4..2676d3fc0f 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -13,6 +13,27 @@ #include "Parameters.h" #include "ch.h" +#ifndef CAN_APP_VERSION_MAJOR +#define CAN_APP_VERSION_MAJOR 1 +#endif +#ifndef CAN_APP_VERSION_MINOR +#define CAN_APP_VERSION_MINOR 0 +#endif + +/* + app descriptor compatible with MissionPlanner + */ +struct app_descriptor { + uint8_t sig[8] = { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 }; + uint32_t image_crc1 = 0; + uint32_t image_crc2 = 0; + uint32_t image_size = 0; + uint32_t git_hash = 0; + uint8_t version_major = CAN_APP_VERSION_MAJOR; + uint8_t version_minor = CAN_APP_VERSION_MINOR; + uint8_t reserved[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; +}; +extern const struct app_descriptor app_descriptor; class AP_Periph_FW { public: diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 572c043092..1ff016dca3 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -60,12 +60,6 @@ static uint32_t canard_memory_pool[2048/4]; static uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID; static uint8_t transfer_id; -#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 @@ -117,6 +111,11 @@ static void handle_get_node_info(CanardInstance* ins, pkt.status = node_status; pkt.software_version.major = CAN_APP_VERSION_MAJOR; pkt.software_version.minor = CAN_APP_VERSION_MINOR; + pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT | UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC; + pkt.software_version.vcs_commit = app_descriptor.git_hash; + uint32_t *crc = (uint32_t *)&pkt.software_version.image_crc; + crc[0] = app_descriptor.image_crc1; + crc[1] = app_descriptor.image_crc2; readUniqueID(pkt.hardware_version.unique_id); @@ -555,6 +554,10 @@ static void can_safety_button_update(void) static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { +#ifdef HAL_GPIO_PIN_LED_CAN1 + palToggleLine(HAL_GPIO_PIN_LED_CAN1); +#endif + /* * Dynamic node ID allocation protocol. * Taking this branch only if we don't have a node ID, ignoring otherwise.