emros/helper/inc/bringup_helper.h

@@ -25,6 +25,7 @@ public:
 	Error BootMotors(void);
 	Error CanSetup(void);
 	void TriggerDisplayRefresh(void);
+	void PrintFirmwareVersion(const SdoHandler::NodeId node_id);
 private:
 	OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
 	Timeout<uint32_t, uint32_t, uint32_t (*)()> timeout_PowerboardBringup;

emros/helper/inc/sequential_sdo_helper.h

@@ -5,8 +5,7 @@
  *      Author: sponfeldner_c
  */
 
-#ifndef EMROS_HELPER_INC_SEQUENTIALSDOHELPER_H_
-#define EMROS_HELPER_INC_BRINGUPHELPER_H_
+#pragma once
 
 #include "../../NanoOFC/core/inc/nano_ofc.h"
 #include "../../NanoOFC/core/inc/sdo_handler.h"
@@ -26,6 +25,7 @@ public:
 	using DataLen = uint8_t;
 
 	Error sdoWrite(const NodeId nodeId, const Index index, const Subindex subindex, const uint8_t write_value);
+	Error sdoRead(const NodeId nodeId, const Index index, const Subindex subindex, char readout[], const uint8_t size);
 
 private:
 	OFCLoggerRTClient& myLogger { *OFCLoggerRTClient::instance() };
@@ -35,5 +35,3 @@ private:
 	Error checkCmSDOErrors(general_result_t);
 
 };
-
-#endif /* EMROS_HELPER_INC_BRINGUPHELPER_H_ */

emros/helper/src/bringup_helper.cpp

@@ -7,7 +7,7 @@
 //#ifdef T
 #include "../inc/bringup_helper.h"
 
-namespace{
+namespace {
 	const uint32_t timeout_BootupPowerboard_us = 3E6;
 	const uint32_t timeout_can_reset_us = 3E6;
 	const uint32_t assumed_duration_node_bootup_ms = 2E3;
@@ -25,6 +25,9 @@ namespace{
 	const uint8_t qswitch_on = 1;
 	const uint8_t qswitch_off = 0;
 	const uint32_t max_resets_can_bootup = 2;
+	const uint8_t size_of_firmware_version_string = 32;
+	const SdoHandler::Index idx_manufacturer_software_version = 0x100A;
+	const SdoHandler::Subindex sub_manufacturer_software_version = 0;
 }
 
 
@@ -168,6 +171,7 @@ Error BringupHelper::BootNode(const char* name, const SdoHandler::NodeId node_id
 		if (CheckIfNodeBooted(node_id, timeout_can_reset_us) == Error::NO_ERROR) {
 			myLogger.InsertInfo("Node ",node_id," (",name,") communication booted successfully.");
 			TriggerDisplayRefresh();
+			this->PrintFirmwareVersion(node_id);
 			return Error::NO_ERROR;
 		} else {
 			myLogger.InsertError("Node ",node_id," (",name,") communication boot-up failed.");
@@ -178,3 +182,17 @@ Error BringupHelper::BootNode(const char* name, const SdoHandler::NodeId node_id
 
 }
 
+void BringupHelper::PrintFirmwareVersion(const SdoHandler::NodeId node_id){
+	char manufacturerString[size_of_firmware_version_string];
+	if (sdoHelper.sdoRead(node_id,idx_manufacturer_software_version,sub_manufacturer_software_version,
+			manufacturerString,size_of_firmware_version_string) == Error::NO_ERROR) {
+		myLogger.InsertInfo("FW-version NodeID ", node_id, ": FIR-v",
+				manufacturerString[5], manufacturerString[6],manufacturerString[7],manufacturerString[8]);
+		TriggerDisplayRefresh();
+	}else{
+		myLogger.InsertError("Could not read out firmware version of Node-ID ", node_id);
+		TriggerDisplayRefresh();
+	}
+
+}
+

emros/helper/src/sequential_sdo_helper.cpp

@@ -43,6 +43,27 @@ Error SequentialSdoHelper::sdoWrite(const NodeId nodeId, const Index index, cons
 }
 
 
+Error SequentialSdoHelper::sdoRead(const NodeId nodeId, const Index index, const Subindex subindex, char readout[], const uint8_t size) {
+	sdo_handle_t sdoHandle;
+	sdoHandle = Can::SdoOpen(nodeId);
+	if (sdoHandle == 0)
+	    {
+			myLogger.InsertError(__PRETTY_FUNCTION__, "Sdo-Write failed. Handle was zero.");
+			return (Error::ERROR);
+	    }
+
+	can_result_t can_result = Can::SdoUpload(sdoHandle, index, subindex, readout, size, use_blocking_mode);
+	if (checkCmSDOErrors(can_result) == Error::ERROR) {
+		Can::SdoClose(sdoHandle, true);
+		OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__, " SDO-read request failed.");
+		return (Error::ERROR);
+	}else{
+    	Can::SdoClose(sdoHandle, true);
+    	return (Error::NO_ERROR);
+	}
+}
+
+
 Error SequentialSdoHelper::checkCmSDOErrors(general_result_t cmState)
 {
 	auto cm_error = CanOpenErrors::CmSDOErrors::_from_integral_nothrow(cmState);

emros/main.cpp

@@ -78,6 +78,7 @@ void RealtimeTask(void) {
 	if (bringupHelper.BootMotors() == Error::ERROR) {
 		myLogger.InsertError("There is something wrong, not all motors booted");
 	}
+
 	//-----------------------------EMROS SETUP--------------------------------//
 
 #ifdef DEF_4WD

emros/sd-card/2WD_plus_auxiliary/dcfs/3_auxiliary_PD_motor.dcf

@@ -1407,3 +1407,23 @@ AccessType=rww
 DefaultValue=6000
 PDOMapping=1
 ParameterValue=200000
+
+
+
+[2061]
+ParameterName= Velocity Numerator
+ObjectType=0x7
+DataType=0x0007
+AccessType=rww
+DefaultValue=0x01
+PDOMapping=1
+ParameterValue=1000000
+
+[2062]
+ParameterName= Velocity Denominator
+ObjectType=0x7
+DataType=0x0007
+AccessType=rww
+DefaultValue=0x01
+PDOMapping=1
+ParameterValue=3141592654