I have a few more answers and candumps.
The kocherga CRC not matching between kocherga_image.py
and the kocherga boot loader itself seems to be some sort of memory layout issue as I can change what function breaks the application image by commenting out a different function.
I tried a variety of GetInfo Server configurations (adding or not adding the name
and software_image_crc
fields, as well as using the NodeInfo
class).
By commenting and uncommenting out the defines at the top I observed and gathered candumps with different sections of code present or not. If the configuration booted into the app, I started a candump and sent two requests for GetInfo (yakut call 111 430:uavcan.node.GetInfo
twice).
Source code for the whole repo is here.
Here is the file at question in full (a modified version of the Cyphal++ blink example):
/*
* This example creates a OpenCyphal node. The builtin LED can be switched on and off using OpenCyphal.
* It also shows periodic transmission of a OpenCyphal heartbeat message via CAN.
*
* switch built in LED on with
* yakut pub 1620:uavcan.primitive.scalar.Bit.1.0 'value: true'
*
* switch built in LED off with
* yakut pub 1620:uavcan.primitive.scalar.Bit.1.0 'value: false'
*/
/**************************************************************************************
* INCLUDE
**************************************************************************************/
#define EXEC_COMMAND
// #define HOMEBREW_GET_INFO
// #define HOMEBREW_ADD_CRC
// #define HOMEBREW_ADD_NAME
#include <107-Arduino-Cyphal.h>
#include "appDesc.hpp"
#include "argsFromApp.hpp"
extern "C" {
#include "crossCoreCan2040.h"
}
#include "RP2040.h"
#include <stdlib.h>
#include <stdint.h>
#include "hardware/watchdog.h"
#include "hardware/sync.h"
#include "pico/stdlib.h"
/**************************************************************************************
* NAMESPACE
**************************************************************************************/
using namespace uavcan::node;
using namespace uavcan::primitive::scalar;
/**************************************************************************************
* CONSTANTS
**************************************************************************************/
static int const CAN2040_RX_PIN = 4;
static int const CAN2040_TX_PIN = 5;
static CanardPortID const BIT_PORT_ID = 1620U;
/**************************************************************************************
* FUNCTION DECLARATION
**************************************************************************************/
#ifdef EXEC_COMMAND
ExecuteCommand::Response_1_1 onExecuteCommand_1_1_Request_Received(ExecuteCommand::Request_1_1 const &, cyphal::TransferMetadata const &);
#endif
#ifdef HOMEBREW_GET_INFO
GetInfo::Response_1_0 onGetInfo_1_0_Request_Received(GetInfo::Request_1_0 const &, cyphal::TransferMetadata const &);
#endif
void onBit_1_0_Received (Bit_1_0 const & msg);
void picoRestart()
{
// Reset the watchdog timer
watchdog_enable(1, 0);
while(1) {
tight_loop_contents(); // Literally a no-op, but the one the SDK uses
}
}
bool repeating_timer_callback(struct repeating_timer *t) {
watchdog_update();
return true;
}
/**************************************************************************************
* GLOBAL VARIABLES
**************************************************************************************/
cyphal::Node::Heap<cyphal::Node::DEFAULT_O1HEAP_SIZE> node_heap __attribute__((used, section(".o1heap")));
cyphal::Node node_hdl(node_heap.data(), node_heap.size(), time_us_64, [] (CanardFrame const & frame) { crossCoreCanSend(&frame); return true; }, 111);
#ifndef HOMEBREW_GET_INFO
cyphal::NodeInfo node_info = node_hdl.create_node_info(
CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR, CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR,
0, 1,
SOFTWARE_VERSION_MAJOR, SOFTWARE_VERSION_MINOR,
GIT_HASH,
std::array<uint8_t, 16>{162, 48, 41, 219, 193, 236, 162, 114, 154, 55, 191, 50, 166, 35, 204, 163},
"org.cwrubaja.pico.testboard"
);
#endif
cyphal::Publisher<Heartbeat_1_0> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0>
(1*1000*1000UL /* = 1 sec in usecs. */);
cyphal::Subscription bit_subscription = node_hdl.create_subscription<Bit_1_0>
(BIT_PORT_ID, onBit_1_0_Received);
#ifdef EXEC_COMMAND
cyphal::ServiceServer execute_command_srv = node_hdl.create_service_server<ExecuteCommand::Request_1_1, ExecuteCommand::Response_1_1>(
2*1000*1000UL,
onExecuteCommand_1_1_Request_Received);
#endif
#ifdef HOMEBREW_GET_INFO
cyphal::ServiceServer get_info_srv = node_hdl.create_service_server<GetInfo::Request_1_0, GetInfo::Response_1_0>(
10*1000*1000UL,
onGetInfo_1_0_Request_Received);
#endif
static const volatile AppDescriptor g_app_descriptor __attribute__((used, section(".app_descriptor")));
/******************************************5********************************************
* MAIN
**************************************************************************************/
int main() {
gpio_init(PICO_DEFAULT_LED_PIN);
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
crossCoreCanInit(1'000'000, CAN2040_RX_PIN, CAN2040_TX_PIN);
// struct repeating_timer timer;
// add_repeating_timer_ms(-250, repeating_timer_callback, NULL, &timer);
// watchdog_enable(500, 0);
while (true) {
/* Process all pending OpenCyphal actions.
*/
CanardFrame frame;
uint8_t payload[8];
frame.payload = payload;
// Empty Queue of received frames
while(crossCoreCanReceive(&frame)) {
node_hdl.onCanFrameReceived(frame);
}
node_hdl.spinSome();
static uint_fast64_t prev = 0;
uint_fast64_t now = time_us_64();
/* Publish the heartbeat once/second */
if(now - prev > 1'000'000)
{
prev = now;
uavcan::node::Heartbeat_1_0 msg;
msg.uptime = now / 1'000'000UL;
msg.health.value = uavcan::node::Health_1_0::NOMINAL;
msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL;
msg.vendor_specific_status_code = 0;
heartbeat_pub->publish(msg);
}
// __wfe();
}
}
/**************************************************************************************
* FUNCTION DEFINITION
**************************************************************************************/
void onBit_1_0_Received(Bit_1_0 const & msg)
{
gpio_put(PICO_DEFAULT_LED_PIN, msg.value);
}
#ifdef EXEC_COMMAND
ExecuteCommand::Response_1_1 onExecuteCommand_1_1_Request_Received(ExecuteCommand::Request_1_1 const & req, cyphal::TransferMetadata const & metadata)
{
ExecuteCommand::Response_1_1 rsp;
std::string parameter;
if (req.command == uavcan::node::ExecuteCommand::Request_1_1::COMMAND_BEGIN_SOFTWARE_UPDATE) {
ArgumentsFromApplication args = {
.args_from_app_version_major = 0,
.cyphal_serial_node_id = 0xFF,
.cyphal_can_node_id = node_hdl.getNodeId(),
.file_server_node_id = metadata.remote_node_id
};
std::copy(req.parameter.begin(), req.parameter.end(), args.remote_file_path.begin());
VolatileStorage<ArgumentsFromApplication> volatileStore(reinterpret_cast<std::uint8_t*>(ARGS_ADDR));
volatileStore.store(args);
picoRestart();
} else if (req.command == 0xCAFE) {
rsp.status = ExecuteCommand::Response_1_1::STATUS_SUCCESS;
} else {
rsp.status = uavcan::node::ExecuteCommand_1_1::Response::STATUS_BAD_COMMAND;
}
return rsp;
}
#endif
#ifdef HOMEBREW_GET_INFO
GetInfo::Response_1_0 onGetInfo_1_0_Request_Received(GetInfo::Request_1_0 const & req, cyphal::TransferMetadata const & metadata)
{
GetInfo::Response_1_0 rsp = {
.protocol_version = {
.major = CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR,
.minor = CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR
},
.hardware_version = {
.major = 0,
.minor = 1
},
.software_version = {
.major = SOFTWARE_VERSION_MAJOR,
.minor = SOFTWARE_VERSION_MINOR
},
.software_vcs_revision_id = GIT_HASH,
.unique_id = {
2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2
} // #TODO: Use Flash Unique ID
};
#ifdef HOMEBREW_ADD_CRC
std::array<std::uint64_t, 1> const CRC = {g_app_descriptor.image_crc};
printf("CRC: %lu\n", CRC[0]);
std::copy(CRC.cbegin(), CRC.cend(), std::back_inserter(rsp.software_image_crc));
#endif
#ifdef HOMEBREW_ADD_NAME
std::array<std::uint8_t, 30> const name = {"org.cwrubaja.pico.testboard"};
printf("name: %lu\n", name);
std::copy(name.cbegin(), name.cend(), std::back_inserter(rsp.name));
#endif
return rsp;
}
#endif
The following matrix summarized those results:
βββββββββββββββββββββββββββ¬ββββββββββββββββββββββββββ
β β β
β EXEC_COMM Undefined β EXEC_COMM Defined β
β β β
βββββββββββ βββββββββββββββββββββββββββΌββββββββββββββββββββββββββ€
βHOMEBREW_β β β App boots β
βGET_INFO β β Kocherga CRC does not β GetInfo Requests β
βUndefinedβ βmatch, app does not boot β fail/timeout in Yakut β
β β β β β
βNodeInfo β β β NodeInfo.log β
βββββββββββ€ ββββββββββββββ¬βββββββββββββΌβββββββββββββ¬βββββββββββββ€
β β βHOMEBREW_ADDβHOMEBREW_ADDβHOMEBREW_ADDβHOMEBREW_ADDβ
β β β _NAME β _NAME β _NAME β _NAME β
β β β Undefined β Defined β Undefined β Defined β
β βββββββββββΌβββββββββββββΌβββββββββββββΌβββββββββββββΌβββββββββββββ€
β β β App Boots, β β β β
β β β GetInfo β App Boots, β App Boots, β β
β βHOMEBREW_β Succeeds, β GetInfo β GetInfo β β
βHOMEBREW_β CRC β Yakut β Fails β Succeeds β β
βGET_INFO βUndefinedβ complains β β β β
β Defined β β βHomeBrew-no-βHomeBrew-no-β β
β β βHomeBrew-no-β yes.log β no.log βKocherga CRCβ
β My β β no.txt β β β does not β
βFunction βββββββββββΌβββββββββββββΌβββββββββββββΌβββββββββββββ€ match, app β
β is Used β β β β β does not β
β β β App Boots, β App Boots, β App Boots, β Boot β
β β β GetInfo β GetInfo β GetInfo β β
β βHOMEBREW_β Fails β Fails β Fails β β
β β CRC β β β β β
β β Defined β βHomeBrew-yesβHomeBrew-yesβ β
β β β β -yes.log β -no.log β β
β β β β β β β
β β β β β β β
βββββββββββ΄ββββββββββ΄βββββββββββββ΄βββββββββββββ΄βββββββββββββ΄βββββββββββββ
Attached are four candump .log
files from each configuration in which rectangle they reside.
Also attached, is a .txt
file of the yakut command and warning sometimes produced in the successful configuration.
Let me know if you need me to figure out the memory layout, kocherga app not booting issue before youβre willing to look at the candumps. However, I believe I can sidestep that issue for the time being by toggling the ExecuteCommand server.
HomeBrew-no-no.log (8.9 KB)
HomeBrew-no-yes.log (4.1 KB)
HomeBrew-yes-no.log (6.4 KB)
HomeBrew-yes-yes.log (4.6 KB)
NodeInfo.log (3.9 KB)
HomeBrew-no-no.txt (1.3 KB)