diff --git a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino index 367ff911..2dea5b6e 100644 --- a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino +++ b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino @@ -49,7 +49,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); +Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 28); ServiceClient srv_client = node_hdl.create_service_client( 2*1000*1000UL, @@ -63,6 +63,8 @@ void setup() { Serial.begin(9600); while(!Serial) { } + delay(1000); + Serial.println("|---- OpenCyphal Service Client Example ----|"); /* Setup SPI access */ SPI.begin(); @@ -78,17 +80,7 @@ void setup() mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); mcp2515.setNormalMode(); - /* Request some coffee. */ - std::string const cmd_param("I want a double espresso with cream!"); - ExecuteCommand::Request_1_1 req; - req.command = 0xCAFE; - std::copy_n(cmd_param.begin(), - std::min(cmd_param.length(), req.parameter.capacity()), - req.parameter.begin()); - - - if (!srv_client->request(27 /* remote node id */, req)) - Serial.println("Coffee request failed."); + Serial.println("setup finished"); } void loop() @@ -99,6 +91,26 @@ void loop() CriticalSection crit_sec; node_hdl.spinSome(); } + + /* Publish the request once/second */ + static unsigned long prev = 0; + unsigned long const now = millis(); + if(now - prev > 1000) + { + prev = now; + + /* Request some coffee. */ + Serial.println("Requesting some coffee"); + std::string const cmd_param("I want a double espresso with cream!"); + ExecuteCommand::Request_1_1 req; + req.command = 0xCAFE; + std::copy(cmd_param.begin(), cmd_param.end(), std::back_inserter(req.parameter)); + + + if (!srv_client->request(27 /* remote node id */, req)) { + Serial.println("Coffee request failed."); + } + } } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino index 41f3f014..e5e8c7cd 100644 --- a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino +++ b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino @@ -49,7 +49,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); +Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 27); ServiceServer execute_command_srv = node_hdl.create_service_server( 2*1000*1000UL, @@ -63,9 +63,12 @@ void setup() { Serial.begin(9600); while(!Serial) { } + delay(1000); + Serial.println("|---- OpenCyphal Service Server Example ----|"); /* Setup SPI access */ SPI.begin(); + pinMode(MKRCAN_MCP2515_CS_PIN, OUTPUT); digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); @@ -77,6 +80,8 @@ void setup() mcp2515.begin(); mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); mcp2515.setNormalMode(); + + Serial.println("setup finished"); } void loop() @@ -101,6 +106,19 @@ void onReceiveBufferFull(CanardFrame const & frame) ExecuteCommand::Response_1_1 onExecuteCommand_1_1_Request_Received(ExecuteCommand::Request_1_1 const & req) { ExecuteCommand::Response_1_1 rsp; + Serial.println("Coffee has been requested"); + + std::string parameter; + + std::copy( + req.parameter.begin(), + req.parameter.end(), + std::back_inserter(parameter)); + + for (uint8_t i = 0; i < parameter.size(); i++) { + Serial.print(parameter[i]); + } + Serial.println(""); if (req.command == 0xCAFE) rsp.status = ExecuteCommand::Response_1_1::STATUS_SUCCESS;