diff --git a/sources/inc/constants.h b/sources/inc/constants.h
index 196fcf8f8c3600d7e8e42cfce77d5f6755587df3..e4b5799764e76666440f8e8297af11a71c6c1737 100644
--- a/sources/inc/constants.h
+++ b/sources/inc/constants.h
@@ -12,43 +12,42 @@
#include <stdint.h>
+#define MCU_32_BIT_DATA_UNDEFINED 0xFFFFFFFF
+#define MCU_16_BIT_DATA_UNDEFINED 0xFFFF
+#define MCU_8_BIT_DATA_UNDEFINED 0xFF
-#define MCU_32_BIT_DATA_UNDEFINED 0xFFFFFFFF
-#define MCU_16_BIT_DATA_UNDEFINED 0xFFFF
-#define MCU_8_BIT_DATA_UNDEFINED 0xFF
-
-//TODO. Now here - later ?
-#define UNUSED(X) (void)X /* To avoid gcc/g++ warnings */
+// TODO. Now here - later ?
+#define UNUSED(X) (void)X /* To avoid gcc/g++ warnings */
typedef enum
{
- SpErrorOk = 0,
- SpErrorRead = 1,
- SpErrorWrite = 2,
- SpErrorInvalid = 3,
- SpErrorNoMemory = 4,
- SpErrorParameter = 5,
- SpErrorUnknown = 99
+ SpErrorOk = 0,
+ SpErrorRead = 1,
+ SpErrorWrite = 2,
+ SpErrorInvalid = 3,
+ SpErrorNoMemory = 4,
+ SpErrorParameter = 5,
+ SpErrorUnknown = 99
} SpError;
typedef enum
{
- MCU_ERROR_NONE = 0,
- MCU_ERROR_NOT_SUPPORTED = 1,
- MCU_ERROR_NOT_FOUND = 2,
- MCU_ERROR_TIMEOUT = 3,
- MCU_ERROR_OVERFLOW = 4,
- MCU_ERROR_UNDERFLOW = 5,
- MCU_ERROR_NO_MEMORY = 6,
- MCU_ERROR_BUSY = 7,
- MCU_ERROR_NOT_INITIALIZED = 8,
- MCU_ERROR_PARAMETER = 9,
- MCU_ERROR_NO_ACCESS = 10,
- MCU_ERROR_CANCEL = 11,
- MCU_ERROR_NOT_ENABLED = 12,
- MCU_ERROR_NOT_ALLOWED = 13,
- MCU_ERROR_UDP_PEER_LOST = 14,
- MCU_ERROR_GENERAL = 99
+ MCU_ERROR_NONE = 0,
+ MCU_ERROR_NOT_SUPPORTED = 1,
+ MCU_ERROR_NOT_FOUND = 2,
+ MCU_ERROR_TIMEOUT = 3,
+ MCU_ERROR_OVERFLOW = 4,
+ MCU_ERROR_UNDERFLOW = 5,
+ MCU_ERROR_NO_MEMORY = 6,
+ MCU_ERROR_BUSY = 7,
+ MCU_ERROR_NOT_INITIALIZED = 8,
+ MCU_ERROR_PARAMETER = 9,
+ MCU_ERROR_NO_ACCESS = 10,
+ MCU_ERROR_CANCEL = 11,
+ MCU_ERROR_NOT_ENABLED = 12,
+ MCU_ERROR_NOT_ALLOWED = 13,
+ MCU_ERROR_UDP_PEER_LOST = 14,
+ MCU_ERROR_GENERAL = 99
} mcu_error;
/*
@@ -61,15 +60,15 @@ typedef enum
} mcu_actuator;
*/
-typedef uint32_t mcu_actuator;
+typedef uint32_t mcu_actuator;
-#define ACTUATOR_0 0
-#define ACTUATOR_1 1
-#define ACTUATOR_2 2
-#define ACTUATOR_3 3
+#define ACTUATOR_0 0
+#define ACTUATOR_1 1
+#define ACTUATOR_2 2
+#define ACTUATOR_3 3
-#define ACTUATOR_FIRST ACTUATOR_0
-#define ACTUATOR_LAST ACTUATOR_2
-#define MCU_SYSTEM_ACT_MAX_COUNT (ACTUATOR_2 + 1)
+#define ACTUATOR_FIRST ACTUATOR_0
+#define ACTUATOR_LAST ACTUATOR_2
+#define MCU_SYSTEM_ACT_MAX_COUNT (ACTUATOR_2 + 1)
#endif
diff --git a/sources/src/functions.c b/sources/src/functions.c
index 776afa2025888894be519e0db5db72a6f5b92d0e..d207762c56524f1328c6b0c3566952c4c8680338 100644
--- a/sources/src/functions.c
+++ b/sources/src/functions.c
@@ -208,9 +208,9 @@ void *readUdp(void *arg)
struct sockaddr recv_addr;
// Set a timeout for the recvfrom function
- struct timeval timeout;
- timeout.tv_sec = 2; // 5 seconds timeout
- timeout.tv_usec = 0;
+ // struct timeval timeout;
+ // timeout.tv_sec = 2; // 5 seconds timeout
+ // timeout.tv_usec = 0;
// if (setsockopt(udp_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0)
// {
@@ -224,7 +224,7 @@ void *readUdp(void *arg)
int bytes_read = (int)recvfrom(udp_sock, buffer, MAX_BUFFER_SIZE, 0, &recv_addr, &len);
if (bytes_read > 0 && !isOwnIp(&recv_addr))
{
- printf("Controller message recieved %d\n", bytes_read);
+ // printf("Controller message recieved %d\n", bytes_read);
processSmcp1Request(buffer, &recv_addr, bytes_read);
}
else if (bytes_read < 0)
@@ -235,7 +235,7 @@ void *readUdp(void *arg)
// }
// else
// {
- perror("recvfrom failed");
+ // perror("recvfrom failed");
if (PROXY_INIT_COMPLETE)
{
printf("ERROR : UDP-SOCKET COMMUNICATION FAILED, CLOSING PROGRAM\n");
diff --git a/sources/src/integration.c b/sources/src/integration.c
index bec826a2ed3918d01b1f281c3c1fb57e30e2eca8..7635660daa0d5817a534967db452a2bb0c108609 100644
--- a/sources/src/integration.c
+++ b/sources/src/integration.c
@@ -23,7 +23,8 @@ static int32_t gAxisMemTarget[MCU_SYSTEM_ACT_MAX_COUNT][2] = {0};
static uint32_t gAxisSpeed[MCU_SYSTEM_ACT_MAX_COUNT] = {0};
static uint16_t gIntegrationDeviceId = 0;
-mcu_error integrationInitActuator(mcu_actuator act) {
+mcu_error integrationInitActuator(mcu_actuator act)
+{
tmclReq tmclRequest;
tmclResp tmclResponse = {0};
@@ -36,15 +37,19 @@ mcu_error integrationInitActuator(mcu_actuator act) {
tmclRequest.checkSum = tmclCalculateCheckSumReq(&tmclRequest);
mcu_error retVal = sendTcmlRequest(&tmclRequest, &tmclResponse);
- if (retVal == MCU_ERROR_NONE) {
+ if (retVal == MCU_ERROR_NONE)
+ {
// Update the position
tmclSetPosition(act, (int32_t)tmclResponse.data);
- } else {
+ }
+ else
+ {
retVal = MCU_ERROR_NOT_INITIALIZED;
}
// Set max positioning speed
- if (retVal == MCU_ERROR_NONE) {
+ if (retVal == MCU_ERROR_NONE)
+ {
tmclRequest.moduleAddr = act;
tmclRequest.cmd = tmclCmdSetAxisParameter;
tmclRequest.type = tmclAxisParamMaxPositioningSpeed;
@@ -58,20 +63,22 @@ mcu_error integrationInitActuator(mcu_actuator act) {
uint16_t integrationGetDev()
{
- if (gIntegrationDeviceId == 0) {
+ if (gIntegrationDeviceId == 0)
+ {
// Fetch from the device
tmclResp tmclResponse;
tmclReq tmclRequest;
tmclRequest.cmd = tmclCmdGetGlobalParameter;
- tmclRequest.addr = 0; //Serial address
+ tmclRequest.addr = 0; // Serial address
tmclRequest.moduleAddr = 0;
tmclRequest.type = tmclGlobalParamSerialAddress;
tmclRequest.data = 0;
tmclRequest.checkSum = tmclCalculateCheckSumReq(&tmclRequest);
mcu_error error = sendTcmlRequest(&tmclRequest, &tmclResponse);
- if (error == MCU_ERROR_NONE) {
+ if (error == MCU_ERROR_NONE)
+ {
gIntegrationDeviceId = tmclResponse.data;
}
}
@@ -81,20 +88,22 @@ uint16_t integrationGetDev()
uint16_t integrationSetDev(uint16_t devId)
{
- if (devId != gIntegrationDeviceId && devId < 255) {
+ if (devId != gIntegrationDeviceId && devId < 255)
+ {
// Fetch from the device
tmclResp tmclResponse;
tmclReq tmclRequest;
tmclRequest.cmd = tmclCmdSetGlobalParameter;
- tmclRequest.addr = 0; //Serial address
+ tmclRequest.addr = 0; // Serial address
tmclRequest.moduleAddr = 0;
tmclRequest.type = tmclGlobalParamSerialAddress;
tmclRequest.data = devId;
tmclRequest.checkSum = tmclCalculateCheckSumReq(&tmclRequest);
mcu_error error = sendTcmlRequest(&tmclRequest, &tmclResponse);
- if (error == MCU_ERROR_NONE) {
+ if (error == MCU_ERROR_NONE)
+ {
gIntegrationDeviceId = tmclResponse.data;
}
}
@@ -102,10 +111,11 @@ uint16_t integrationSetDev(uint16_t devId)
return gIntegrationDeviceId;
}
-char * integrationGetFirmwareVersion()
+char *integrationGetFirmwareVersion()
{
- static char firmwareVersion[100] = {'\0'};
- if(strlen(firmwareVersion) == 0) {
+ static char firmwareVersion[100] = {'\0'};
+ if (strlen(firmwareVersion) == 0)
+ {
// Fetch from the device
tmclResp tmclResponse;
@@ -118,7 +128,8 @@ char * integrationGetFirmwareVersion()
tmclRequest.checkSum = tmclCalculateCheckSumReq(&tmclRequest);
mcu_error error = sendTcmlRequest(&tmclRequest, &tmclResponse);
- if (error == MCU_ERROR_NONE) {
+ if (error == MCU_ERROR_NONE)
+ {
uint16_t model = tmclResponse.data >> 16;
uint8_t versionMajor = (tmclResponse.data >> 8) & 0xff;
uint8_t versionMinor = tmclResponse.data & 0xff;
@@ -138,31 +149,40 @@ mcu_error integrationTakeLegacyStep(mcu_actuator act, int32_t gear, int32_t spee
{
int32_t tmcSpeed = integrationGetLegacySpeed(gear);
int32_t stepTicks = integrationGetLegacyStepLength(gear);
- if (speed < 0) {
+ if (speed < 0)
+ {
stepTicks = -stepTicks;
}
mcu_error retVal = integrationsDoTakeStep(act, tmcSpeed, stepTicks, false, false);
return retVal;
}
-mcu_error integrationSetMemDriveTarget(mcu_actuator act, uint32_t memIndex, int32_t position) {
+mcu_error integrationSetMemDriveTarget(mcu_actuator act, uint32_t memIndex, int32_t position)
+{
mcu_error retVal = MCU_ERROR_NONE;
memIndex--;
- if (memIndex < 2) {
+ if (memIndex < 2)
+ {
gAxisMemTarget[act][memIndex] = position;
- } else {
+ }
+ else
+ {
retVal = MCU_ERROR_PARAMETER;
}
return retVal;
}
-mcu_error integrationGetMemDriveTarget(mcu_actuator act, uint32_t memIndex, int32_t* position) {
+mcu_error integrationGetMemDriveTarget(mcu_actuator act, uint32_t memIndex, int32_t *position)
+{
mcu_error retVal = MCU_ERROR_NONE;
memIndex--;
- if (memIndex < 2) {
+ if (memIndex < 2)
+ {
*position = gAxisMemTarget[act][memIndex];
- } else {
+ }
+ else
+ {
retVal = MCU_ERROR_PARAMETER;
}
return retVal;
@@ -200,32 +220,31 @@ mcu_error integrationGetPosition(mcu_actuator act, int *position, bool rawData)
int32_t integrationGetLegacySpeed(int gear)
{
int32_t speed = 0;
- switch(gear)
+ switch (gear)
{
- case 1:
- speed = 1;
- break;
- case 2:
- speed = 5;
- break;
- case 3:
- speed = 25;
- break;
- case 4:
- speed = 125;
- break;
- case 5:
- speed = 500;
- break;
- case 6:
- speed = 1500;
- break;
- case 7: // pen mode
- speed = 2000;
- break;
- default:
- assert(false);
-
+ case 1:
+ speed = 1;
+ break;
+ case 2:
+ speed = 5;
+ break;
+ case 3:
+ speed = 25;
+ break;
+ case 4:
+ speed = 125;
+ break;
+ case 5:
+ speed = 500;
+ break;
+ case 6:
+ speed = 1500;
+ break;
+ case 7: // pen mode
+ speed = 2000;
+ break;
+ default:
+ assert(false);
}
return speed;
}
@@ -233,31 +252,31 @@ int32_t integrationGetLegacySpeed(int gear)
int32_t integrationGetLegacyStepLength(int gear)
{
int32_t step = 0;
- switch(gear)
+ switch (gear)
{
- case 1:
- step = 1;
- break;
- case 2:
- step = 5;
- break;
- case 3:
- step = 25;
- break;
- case 4:
- step = 125;
- break;
- case 5:
- step = 500;
- break;
- case 6:
- step = 1000;
- break;
- case 7: // pen mode
- step = 500;
- break;
- default:
- assert(false);
+ case 1:
+ step = 1;
+ break;
+ case 2:
+ step = 5;
+ break;
+ case 3:
+ step = 25;
+ break;
+ case 4:
+ step = 125;
+ break;
+ case 5:
+ step = 500;
+ break;
+ case 6:
+ step = 1000;
+ break;
+ case 7: // pen mode
+ step = 500;
+ break;
+ default:
+ assert(false);
}
return step;
}
@@ -270,7 +289,8 @@ mcu_error integrationsDoTakeStep(mcu_actuator act, int32_t speed, int32_t target
tmclResp tmclResponse;
// Set the target speed
- if (speed != gAxisSpeed[act]) {
+ if (speed != gAxisSpeed[act])
+ {
tmclRequest.addr = integrationGetDev();
tmclRequest.moduleAddr = act;
tmclRequest.cmd = tmclCmdSetAxisParameter;
@@ -279,13 +299,15 @@ mcu_error integrationsDoTakeStep(mcu_actuator act, int32_t speed, int32_t target
tmclRequest.checkSum = tmclCalculateCheckSumReq(&tmclRequest);
retVal = sendTcmlRequest(&tmclRequest, &tmclResponse);
- if (retVal == MCU_ERROR_NONE) {
+ if (retVal == MCU_ERROR_NONE)
+ {
gAxisSpeed[act] = speed;
}
}
// Take the step
- if (retVal == MCU_ERROR_NONE) {
+ if (retVal == MCU_ERROR_NONE)
+ {
tmclRequest.addr = integrationGetDev();
tmclRequest.moduleAddr = act;
tmclRequest.cmd = tmclCmdMoveToPosition;
@@ -295,8 +317,10 @@ mcu_error integrationsDoTakeStep(mcu_actuator act, int32_t speed, int32_t target
retVal = sendTcmlRequest(&tmclRequest, &tmclResponse);
- if (retVal == MCU_ERROR_NONE) {
- if (waitFinish) {
+ if (retVal == MCU_ERROR_NONE)
+ {
+ if (waitFinish)
+ {
mcu_actuatorSetBusyStatus(act, 1);
// Wait if needed
tmclReq tmclReqTargetReached;
@@ -329,33 +353,43 @@ mcu_error integrationsDoTakeStep(mcu_actuator act, int32_t speed, int32_t target
// Wait unit the motor gets
int stallCounter = 0;
- do {
+ do
+ {
retVal = sendTcmlRequest(&tmclReqActualPosition, &tmclRespActualPosition);
- tmclSetPosition(act, (int32_t) tmclRespActualPosition.data);
+ tmclSetPosition(act, (int32_t)tmclRespActualPosition.data);
- if (retVal == MCU_ERROR_NONE) {
+ if (retVal == MCU_ERROR_NONE)
+ {
retVal = sendTcmlRequest(&tmclReqTargetReached, &tmclRespTargetReached);
}
- if (retVal == MCU_ERROR_NONE && tmclRespTargetReached.data == 1) {
+ if (retVal == MCU_ERROR_NONE && tmclRespTargetReached.data == 1)
+ {
break;
}
retVal = sendTcmlRequest(&tmclReqMotorStopped, &tmclRespMotorStopped);
- if (retVal == MCU_ERROR_NONE) {
- if (tmclRespMotorStopped.data == 0) {
+ if (retVal == MCU_ERROR_NONE)
+ {
+ if (tmclRespMotorStopped.data == 0)
+ {
stallCounter++;
- if (stallCounter >= 10) {
+ if (stallCounter >= 10)
+ {
retVal = MCU_ERROR_CANCEL;
break;
}
- } else {
+ }
+ else
+ {
stallCounter = 0;
}
}
osThreadYield();
} while (retVal == MCU_ERROR_NONE);
- } else {
+ }
+ else
+ {
// Update the position immediately (NOTE: tmcl response value to tmclCmdMoveToPosition is 'don't care')
- tmclSetPosition(act, (int32_t) tmclResponse.data);
+ tmclSetPosition(act, (int32_t)tmclResponse.data);
}
mcu_actuatorSetBusyStatus(act, 0);
}
diff --git a/sources/src/smcp1.c b/sources/src/smcp1.c
index be502f78aa830a4a9dccfc28e7ebd32b23b5af73..34a2a312b7d8a5e5b6e42b4f939e811d85e7990d 100644
--- a/sources/src/smcp1.c
+++ b/sources/src/smcp1.c
@@ -21,15 +21,15 @@
#include <stdio.h>
#include <string.h>
-#define SMCP10_PARAM_COUNT 25
-#define SMCP10_PARAM_DYNAMIC_START 0x80
-#define SMCP10_PARAM_READONLY_START 0x100
-#define SMCP10_PARAM_DYNAMIC_END (SMCP10_PARAM_READONLY_START - 1)
-#define SMCP10_PARAM_READONLY_END 0x200
-#define SMCP10_PARAM_UNINITIALIZED -1
+#define SMCP10_PARAM_COUNT 25
+#define SMCP10_PARAM_DYNAMIC_START 0x80
+#define SMCP10_PARAM_READONLY_START 0x100
+#define SMCP10_PARAM_DYNAMIC_END (SMCP10_PARAM_READONLY_START - 1)
+#define SMCP10_PARAM_READONLY_END 0x200
+#define SMCP10_PARAM_UNINITIALIZED -1
-#define SMCP10_PEER_ADDR_CACHE_SIZE 5
-#define SMCP10_PEER_ADDR_CACHE_TIMEOUT_MS 60000
+#define SMCP10_PEER_ADDR_CACHE_SIZE 5
+#define SMCP10_PEER_ADDR_CACHE_TIMEOUT_MS 60000
typedef struct mcu_smcp1_peer_
{
@@ -41,26 +41,26 @@ typedef struct mcu_smcp1_peer_
static pthread_mutex_t peerMutex = PTHREAD_MUTEX_INITIALIZER;
// Pre-execution option handlers
-static void handle_smcp10_pre_exec_opt_wait_trigger_1(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_priority(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_req_bcast(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_req_notify(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_req_resp(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_error(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_notify(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_ack(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_pre_exec_opt_req(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_wait_trigger_1(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_priority(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_req_bcast(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_req_notify(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_req_resp(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_error(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_notify(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_ack(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_pre_exec_opt_req(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
// Post-execution option handlers
-static void handle_smcp10_post_exec_opt_priority(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_req_bcast(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_req_notify(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_req_resp(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_req_ack(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_error(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_notify(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_ack(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
-static void handle_smcp10_post_exec_opt_req(mcu_event_queue target, mcu_event* pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_priority(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_req_bcast(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_req_notify(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_req_resp(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_req_ack(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_error(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_notify(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_ack(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
+static void handle_smcp10_post_exec_opt_req(mcu_event_queue target, mcu_event *pEvent, mcu_event_status *pStatus);
// Command handlers
static int handle_smcp10_cmd_stop(mcu_event_queue target, mcu_event *pEvent);
@@ -78,13 +78,13 @@ static char *smcpv10_compose_reply(mcu_event *pReqEvent, char *pData, int size,
static int do_handle_smcp10_cmd_goto_mem(const mcu_event_queue target, mcu_event *pEvent, bool reverseOrder);
static mcu_error mcu_smcp10_set_default_params();
-static uint32_t mcu_commonResolveActuatorAxisCount(const systemInfo * info);
+static uint32_t mcu_commonResolveActuatorAxisCount(const systemInfo *info);
static void mcu_smcp10SetLatestError(int32_t error);
static void smcp10_execute_error_handler(mcu_event *pEvent, int error);
static int mcu_smcp10SizeofTypeBase(smcp1_sb_data_type datatype);
-static char* smcpv10_find_element(const mcu_event *pEvent, smcp1_mcu_event_element block);
-static smcp1_frame * getSMCPv1Frame(const mcu_event *pEvent, smcp1_frame *pTarget);
+static char *smcpv10_find_element(const mcu_event *pEvent, smcp1_mcu_event_element block);
+static smcp1_frame *getSMCPv1Frame(const mcu_event *pEvent, smcp1_frame *pTarget);
static mcu_error mcu_smcp10_read_param(uint32_t paramId, int *value, bool refresh_cache);
static mcu_error mcu_smcp10_write_param(uint32_t paramID, int value, bool write_through);
static void mcu_smcp10_hton_frame(smcp1_frame *dest, smcp1_frame *source);
@@ -101,70 +101,73 @@ static pre_exec_handler smcp10_event_pre_exec_options_handlers[MCU_EVENT_OPTIONS
// ... post-execution
static post_exec_handler smcp10_event_post_exec_options_handlers[MCU_EVENT_OPTIONS_MAX_COUNT];
-
mcu_error mcu_smcp10_init()
{
// Clear parameter cache
memset(smcp10_params_cache, 0xFF, sizeof smcp10_params_cache);
// Make sure that there is a meaningful default value for all main parameters
- mcu_error retval = mcu_smcp10_set_default_params();
-
+ mcu_error retval = mcu_smcp10_set_default_params();
return retval;
}
-static int smcp1SizeOfTypeBase(smcp1_sb_data_type datatype) {
+static int smcp1SizeOfTypeBase(smcp1_sb_data_type datatype)
+{
int retVal;
- switch (datatype) {
- case SMCP1_DATA_VOID:
- retVal = sizeof(void);
- break;
- case SMCP1_DATA_UINT8:
- case SMCP1_DATA_UINT8_ARRAY_2:
- case SMCP1_DATA_UINT8_ARRAY_3:
- case SMCP1_DATA_UINT8_ARRAY_4:
- case SMCP1_DATA_CHAR_STRING:
- retVal = sizeof(uint8_t);
- break;
- case SMCP1_DATA_INT8:
- case SMCP1_DATA_INT8_ARRAY_2:
- case SMCP1_DATA_INT8_ARRAY_3:
- case SMCP1_DATA_INT8_ARRAY_4:
- retVal = sizeof(int8_t);
- break;
- case SMCP1_DATA_UINT16:
- case SMCP1_DATA_UINT16_ARRAY_2:
- retVal = sizeof(uint16_t);
- break;
- case SMCP1_DATA_INT16:
- case SMCP1_DATA_INT16_ARRAY_2:
- retVal = sizeof(int16_t);
- break;
- case SMCP1_DATA_UINT32:
- retVal = sizeof(uint32_t);
- break;
- case SMCP1_DATA_INT32:
- retVal = sizeof(int32_t);
- break;
- case SMCP1_DATA_UINT64:
- retVal = sizeof(uint64_t);
- break;
- case SMCP1_DATA_INT64:
- retVal = sizeof(int64_t);
- break;
- default:
- assert(false); // Unknown datatype. Assert it.
+ switch (datatype)
+ {
+ case SMCP1_DATA_VOID:
+ retVal = sizeof(void);
+ break;
+ case SMCP1_DATA_UINT8:
+ case SMCP1_DATA_UINT8_ARRAY_2:
+ case SMCP1_DATA_UINT8_ARRAY_3:
+ case SMCP1_DATA_UINT8_ARRAY_4:
+ case SMCP1_DATA_CHAR_STRING:
+ retVal = sizeof(uint8_t);
+ break;
+ case SMCP1_DATA_INT8:
+ case SMCP1_DATA_INT8_ARRAY_2:
+ case SMCP1_DATA_INT8_ARRAY_3:
+ case SMCP1_DATA_INT8_ARRAY_4:
+ retVal = sizeof(int8_t);
+ break;
+ case SMCP1_DATA_UINT16:
+ case SMCP1_DATA_UINT16_ARRAY_2:
+ retVal = sizeof(uint16_t);
+ break;
+ case SMCP1_DATA_INT16:
+ case SMCP1_DATA_INT16_ARRAY_2:
+ retVal = sizeof(int16_t);
+ break;
+ case SMCP1_DATA_UINT32:
+ retVal = sizeof(uint32_t);
+ break;
+ case SMCP1_DATA_INT32:
+ retVal = sizeof(int32_t);
+ break;
+ case SMCP1_DATA_UINT64:
+ retVal = sizeof(uint64_t);
+ break;
+ case SMCP1_DATA_INT64:
+ retVal = sizeof(int64_t);
+ break;
+ default:
+ assert(false); // Unknown datatype. Assert it.
}
return retVal;
}
-bool smcp1ValidateMessage(const char *p_data, int data_len) {
+bool smcp1ValidateMessage(const char *p_data, int data_len)
+{
bool ret_val = false;
- if (data_len >= sizeof(smcp1_frame)) {
+ if (data_len >= sizeof(smcp1_frame))
+ {
// Protocol version number matches
- if (*p_data == 0x10) {
+ if (*p_data == 0x10)
+ {
// TODO. Contains exactly as much as data it says - etc
ret_val = true;
}
@@ -174,66 +177,71 @@ bool smcp1ValidateMessage(const char *p_data, int data_len) {
// network is 32 bit aligned
// host will be - what ever the current host is (32 bit or 64 bit most likely)
-int smcp1MessageNtoh(const uint8_t *aSource, uint8_t *aTarget, int aSourceLen) {
+int smcp1MessageNtoh(const uint8_t *aSource, uint8_t *aTarget, int aSourceLen)
+{
int retVal = -1;
- if (aSourceLen >= sizeof(smcp1_frame)) {
+ if (aSourceLen >= sizeof(smcp1_frame))
+ {
const uint8_t *pSourceStart = aSource;
// Frame
- smcp1_frame *pFrame = (smcp1_frame *) aTarget;
+ smcp1_frame *pFrame = (smcp1_frame *)aTarget;
- pFrame->version = *(uint8_t *) aSource;
+ pFrame->version = *(uint8_t *)aSource;
aSource += sizeof(uint8_t);
- pFrame->sender_type = *(uint8_t *) aSource;
+ pFrame->sender_type = *(uint8_t *)aSource;
aSource += sizeof(uint8_t);
- pFrame->receiver_id = ntohs(*(uint16_t *) aSource);
+ pFrame->receiver_id = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
- pFrame->sender_id = ntohs(*(uint16_t *) aSource);
+ pFrame->sender_id = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
- pFrame->message_id = ntohs(*(uint16_t *) aSource);
+ pFrame->message_id = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
- pFrame->options = ntohl(*(uint32_t *) aSource);
+ pFrame->options = ntohl(*(uint32_t *)aSource);
aSource += sizeof(uint32_t);
- pFrame->type = ntohs(*(uint16_t *) aSource);
+ pFrame->type = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
- pFrame->sub_blocks = ntohs(*(uint16_t *) aSource);
+ pFrame->sub_blocks = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
- aTarget += sizeof (smcp1_frame);
+ aTarget += sizeof(smcp1_frame);
// Sub blocks
// TODO. Buffer overflow checks
- for (int i = 0; i < pFrame->sub_blocks; i++) {
+ for (int i = 0; i < pFrame->sub_blocks; i++)
+ {
smcp1_sb_header *pSbHeader;
- uint8_t * tmp = (uint8_t*)aTarget;
- pSbHeader = (smcp1_sb_header *) aTarget;
- pSbHeader->type = ntohs(*(uint16_t *) aSource);
+ uint8_t *tmp = (uint8_t *)aTarget;
+ pSbHeader = (smcp1_sb_header *)aTarget;
+ pSbHeader->type = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
- pSbHeader->size = ntohs(*(uint16_t *) aSource);
+ pSbHeader->size = ntohs(*(uint16_t *)aSource);
aSource += sizeof(uint16_t);
aTarget = tmp + sizeof(smcp1_sb_header);
// Process sub block payload.
int elementSize = smcp1SizeOfTypeBase(pSbHeader->type);
- for (int j = 0; j < pSbHeader->size; j++) {
- switch (elementSize) {
- case sizeof(uint8_t):
- *(aTarget) = *(aSource);
- break;
- case sizeof(uint16_t):
- *(uint16_t *) aTarget = ntohs(*(uint16_t *) aSource);
- break;
- case sizeof(uint32_t):
- *(uint32_t *) aTarget = ntohl(*(uint32_t *) aSource);
- break;
- case sizeof(uint64_t):
- *(uint64_t *) aTarget = ntohll_custom(*(uint64_t *) aSource);
- break;
- default:
- assert(false);
+ for (int j = 0; j < pSbHeader->size; j++)
+ {
+ switch (elementSize)
+ {
+ case sizeof(uint8_t):
+ *(aTarget) = *(aSource);
+ break;
+ case sizeof(uint16_t):
+ *(uint16_t *)aTarget = ntohs(*(uint16_t *)aSource);
+ break;
+ case sizeof(uint32_t):
+ *(uint32_t *)aTarget = ntohl(*(uint32_t *)aSource);
+ break;
+ case sizeof(uint64_t):
+ *(uint64_t *)aTarget = ntohll_custom(*(uint64_t *)aSource);
+ break;
+ default:
+ assert(false);
}
aSource += elementSize;
aTarget += elementSize;
@@ -241,135 +249,146 @@ int smcp1MessageNtoh(const uint8_t *aSource, uint8_t *aTarget, int aSourceLen) {
aTarget += elementSize + ((elementSize * pSbHeader->size) % sizeof(int)); // Align to native word size
}
- retVal = (int) (aSource - pSourceStart);
+ retVal = (int)(aSource - pSourceStart);
}
return retVal;
}
-int smcp1MessageHton(const uint8_t *aSource, uint8_t *aTarget, int aSourceLen) {
+int smcp1MessageHton(const uint8_t *aSource, uint8_t *aTarget, int aSourceLen)
+{
int retVal = -1;
- if (aSourceLen >= sizeof(smcp1_frame)) {
+ if (aSourceLen >= sizeof(smcp1_frame))
+ {
const uint8_t *pSourceStart = aSource;
const uint8_t *pTargetStart = aTarget;
// Frame
- smcp1_frame *pFrame = (smcp1_frame *) aSource;
+ smcp1_frame *pFrame = (smcp1_frame *)aSource;
uint16_t subBlockCnt = pFrame->sub_blocks;
- *(uint8_t *) aTarget = pFrame->version;
+ *(uint8_t *)aTarget = pFrame->version;
aTarget += sizeof(uint8_t);
- *(uint8_t *) aTarget = pFrame->sender_type;
+ *(uint8_t *)aTarget = pFrame->sender_type;
aTarget += sizeof(uint8_t);
- *(uint16_t *) aTarget = htons(pFrame->receiver_id);
+ *(uint16_t *)aTarget = htons(pFrame->receiver_id);
aTarget += sizeof(uint16_t);
- *(uint16_t *) aTarget = htons(pFrame->sender_id);
+ *(uint16_t *)aTarget = htons(pFrame->sender_id);
aTarget += sizeof(uint16_t);
- *(uint16_t *) aTarget = htons(pFrame->message_id);
+ *(uint16_t *)aTarget = htons(pFrame->message_id);
aTarget += sizeof(uint16_t);
- *(uint32_t *) aTarget = htonl(pFrame->options);
+ *(uint32_t *)aTarget = htonl(pFrame->options);
aTarget += sizeof(uint32_t);
- *(uint16_t *) aTarget = htons(pFrame->type);
+ *(uint16_t *)aTarget = htons(pFrame->type);
aTarget += sizeof(uint16_t);
- *(uint16_t *) aTarget = htons(pFrame->sub_blocks);
+ *(uint16_t *)aTarget = htons(pFrame->sub_blocks);
aTarget += sizeof(uint16_t);
aSource += sizeof(smcp1_frame);
// Sub blocks
// TODO. Buffer overflow checks
- for (int i = 0; i < subBlockCnt; i++) {
+ for (int i = 0; i < subBlockCnt; i++)
+ {
smcp1_sb_header *pSbHeader;
- pSbHeader = (smcp1_sb_header *) aSource;
+ pSbHeader = (smcp1_sb_header *)aSource;
uint16_t sbDataType = pSbHeader->type;
uint16_t sbDataSize = pSbHeader->size;
- uint8_t * tmp = (uint8_t *)aSource;
+ uint8_t *tmp = (uint8_t *)aSource;
- *(uint16_t *) aTarget = htons(pSbHeader->type);
+ *(uint16_t *)aTarget = htons(pSbHeader->type);
aTarget += sizeof(uint16_t);
- *(uint16_t *) aTarget = htons(pSbHeader->size);
+ *(uint16_t *)aTarget = htons(pSbHeader->size);
aTarget += sizeof(uint16_t);
aSource = tmp + sizeof(smcp1_sb_header);
// Process sub block payload.
int elementSize = smcp1SizeOfTypeBase(sbDataType);
- for (int j = 0; j < sbDataSize; j++) {
- switch (elementSize) {
- case sizeof(uint8_t):
- *(aTarget) = *(aSource);
- break;
- case sizeof(uint16_t):
- *(uint16_t *) aTarget = htons(*(uint16_t *) aSource);
- break;
- case sizeof(uint32_t):
- *(uint32_t *) aTarget = htonl(*(uint32_t *) aSource);
- break;
- case sizeof(uint64_t):
- *(uint64_t *) aTarget = htonll_custom(*(uint64_t *) aSource);
- break;
- default:
- assert(false);
+ for (int j = 0; j < sbDataSize; j++)
+ {
+ switch (elementSize)
+ {
+ case sizeof(uint8_t):
+ *(aTarget) = *(aSource);
+ break;
+ case sizeof(uint16_t):
+ *(uint16_t *)aTarget = htons(*(uint16_t *)aSource);
+ break;
+ case sizeof(uint32_t):
+ *(uint32_t *)aTarget = htonl(*(uint32_t *)aSource);
+ break;
+ case sizeof(uint64_t):
+ *(uint64_t *)aTarget = htonll_custom(*(uint64_t *)aSource);
+ break;
+ default:
+ assert(false);
}
aSource += elementSize;
aTarget += elementSize;
}
}
- assert(((int) (aSource - pSourceStart)) == ((int) (aTarget - pTargetStart)));
+ assert(((int)(aSource - pSourceStart)) == ((int)(aTarget - pTargetStart)));
- retVal = (int) (aTarget - pTargetStart);
+ retVal = (int)(aTarget - pTargetStart);
}
return retVal;
}
-char *smcp1FindElement(const char *pEvent, smcp1_element block, int sbIndex) {
- char *retVal = (char *) pEvent;
+char *smcp1FindElement(const char *pEvent, smcp1_element block, int sbIndex)
+{
+ char *retVal = (char *)pEvent;
int offset;
- switch (block) {
- /*
- case SCMP1_FRAME_ELEM:
- offset = 0;
- break;
- */
- case SCMP1_SUB_BLOCKS_ELEM:
- offset = sizeof(smcp1_frame);
- smcp1_sb_header * pSbHeader = (smcp1_sb_header *)(pEvent + offset);
- // FIXME. Data alignment with multiple sub blocks
- for (int i = 0; i < sbIndex; i++) {
- offset += (int)SMCP1_SUB_BLOCK_HEADER_SIZE + (pSbHeader->size * smcp1SizeOfTypeBase(pSbHeader->type));
- pSbHeader = (smcp1_sb_header *)(pEvent + offset);
- }
- break;
- default:
- assert(false); // Unknown parameter. Assert it.
+ switch (block)
+ {
+ /*
+ case SCMP1_FRAME_ELEM:
+ offset = 0;
+ break;
+ */
+ case SCMP1_SUB_BLOCKS_ELEM:
+ offset = sizeof(smcp1_frame);
+ smcp1_sb_header *pSbHeader = (smcp1_sb_header *)(pEvent + offset);
+ // FIXME. Data alignment with multiple sub blocks
+ for (int i = 0; i < sbIndex; i++)
+ {
+ offset += (int)SMCP1_SUB_BLOCK_HEADER_SIZE + (pSbHeader->size * smcp1SizeOfTypeBase(pSbHeader->type));
+ pSbHeader = (smcp1_sb_header *)(pEvent + offset);
+ }
+ break;
+ default:
+ assert(false); // Unknown parameter. Assert it.
}
retVal += offset;
return retVal;
}
-smcp1_sb_header * smcp1GetSubBlockHeader(const smcp1_frame *pEvent, int sbIndex) {
+smcp1_sb_header *smcp1GetSubBlockHeader(const smcp1_frame *pEvent, int sbIndex)
+{
smcp1_sb_header *retVal = NULL;
char *pSb = smcp1FindElement((const char *)pEvent, SCMP1_SUB_BLOCKS_ELEM, sbIndex);
- if (pSb != NULL) {
+ if (pSb != NULL)
+ {
retVal = (smcp1_sb_header *)pSb;
}
return retVal;
}
-void *smcp1GetSubBlockData(smcp1_sb_header *pSbHdr, uint32_t index) {
+void *smcp1GetSubBlockData(smcp1_sb_header *pSbHdr, uint32_t index)
+{
int itemSize = smcp1SizeOfTypeBase(pSbHdr->type);
- void *pSbDataBlock = (void *) pSbHdr + sizeof(smcp1_sb_header);
- void *retVal = (void *) pSbDataBlock + (index * itemSize);
+ void *pSbDataBlock = (void *)pSbHdr + sizeof(smcp1_sb_header);
+ void *retVal = (void *)pSbDataBlock + (index * itemSize);
return retVal;
}
-static void* smcpv10_get_data(smcp1_sb_header *pSbHdr, char *pSbDataBlock, uint32_t index)
+static void *smcpv10_get_data(smcp1_sb_header *pSbHdr, char *pSbDataBlock, uint32_t index)
{
int itemSize = mcu_smcp10SizeofTypeBase(pSbHdr->type);
- void * retval = (void*)pSbDataBlock + (index * itemSize);
+ void *retval = (void *)pSbDataBlock + (index * itemSize);
return retval;
}
@@ -377,41 +396,40 @@ static int mcu_smcp10NbrOfDataElements(smcp1_sb_data_type datatype)
{
int retval;
- switch(datatype)
+ switch (datatype)
{
- case SMCP1_DATA_VOID:
- case SMCP1_DATA_UINT8:
- case SMCP1_DATA_INT8:
- case SMCP1_DATA_CHAR_STRING:
- case SMCP1_DATA_UINT16:
- case SMCP1_DATA_INT16:
- case SMCP1_DATA_UINT32:
- case SMCP1_DATA_INT32:
- case SMCP1_DATA_UINT64:
- case SMCP1_DATA_INT64:
- retval = 1;
- break;
- case SMCP1_DATA_UINT8_ARRAY_2:
- case SMCP1_DATA_INT8_ARRAY_2:
- case SMCP1_DATA_UINT16_ARRAY_2:
- case SMCP1_DATA_INT16_ARRAY_2:
- retval = 2;
- break;
- case SMCP1_DATA_UINT8_ARRAY_3:
- case SMCP1_DATA_INT8_ARRAY_3:
- retval = 3;
- break;
- case SMCP1_DATA_UINT8_ARRAY_4:
- case SMCP1_DATA_INT8_ARRAY_4:
- retval = 4;
- break;
- default:
- retval = 0;
- assert(0); //Unknown datatype. Assert it.
- break;
+ case SMCP1_DATA_VOID:
+ case SMCP1_DATA_UINT8:
+ case SMCP1_DATA_INT8:
+ case SMCP1_DATA_CHAR_STRING:
+ case SMCP1_DATA_UINT16:
+ case SMCP1_DATA_INT16:
+ case SMCP1_DATA_UINT32:
+ case SMCP1_DATA_INT32:
+ case SMCP1_DATA_UINT64:
+ case SMCP1_DATA_INT64:
+ retval = 1;
+ break;
+ case SMCP1_DATA_UINT8_ARRAY_2:
+ case SMCP1_DATA_INT8_ARRAY_2:
+ case SMCP1_DATA_UINT16_ARRAY_2:
+ case SMCP1_DATA_INT16_ARRAY_2:
+ retval = 2;
+ break;
+ case SMCP1_DATA_UINT8_ARRAY_3:
+ case SMCP1_DATA_INT8_ARRAY_3:
+ retval = 3;
+ break;
+ case SMCP1_DATA_UINT8_ARRAY_4:
+ case SMCP1_DATA_INT8_ARRAY_4:
+ retval = 4;
+ break;
+ default:
+ retval = 0;
+ assert(0); // Unknown datatype. Assert it.
+ break;
}
return retval;
-
}
static int mcu_smcp10SizeofSbPayload(smcp1_sb_header *pSb)
@@ -431,33 +449,37 @@ static int mcu_smcp10SizeofSbPayload(smcp1_sb_header *pSb)
}
static mcu_error mcu_smcpv1_get_sub_block_data(const mcu_event *pEvent, int sbIndex,
- smcp1_sb_header **pDestSb, char **pDestSbData)
+ smcp1_sb_header **pDestSb, char **pDestSbData)
{
mcu_error retval = MCU_ERROR_NOT_FOUND;
- smcp1_sb_header* pSb = (smcp1_sb_header*)
- smcpv10_find_element(pEvent, SCMP1_SUBBLOCK_ELEM);
+ smcp1_sb_header *pSb = (smcp1_sb_header *)
+ smcpv10_find_element(pEvent, SCMP1_SUBBLOCK_ELEM);
int i;
assert(pEvent->frame.sub_blocks >= sbIndex);
- for (i = 0; i < pEvent->frame.sub_blocks; i++) {
+ for (i = 0; i < pEvent->frame.sub_blocks; i++)
+ {
int payloadSize = mcu_smcp10SizeofSbPayload(pSb);
- if (i == sbIndex) {
+ if (i == sbIndex)
+ {
// We found the target sub-block. Set return values and stop.
*pDestSb = pSb;
- *pDestSbData = (char*)((char*)pSb + sizeof(smcp1_sb_header));
+ *pDestSbData = (char *)((char *)pSb + sizeof(smcp1_sb_header));
retval = MCU_ERROR_NONE;
break;
}
- pSb = (smcp1_sb_header *)((void*)pSb + sizeof(smcp1_sb_header) + payloadSize);
+ pSb = (smcp1_sb_header *)((void *)pSb + sizeof(smcp1_sb_header) + payloadSize);
}
return retval;
}
-const char *smcp1GetVersion() {
+const char *smcp1GetVersion()
+{
static char versionBuffer[15] = {'\0'};
- if (versionBuffer[0] == '\0') {
+ if (versionBuffer[0] == '\0')
+ {
sprintf(versionBuffer, "%d.%d.%d.%d", SMCP_PROXY_VERSION_MAJOR, SMCP_PROXY_VERSION_YEAR,
SMCP_PROXY_VERSION_WEEK, SMCP_PROXY_VERSION_DAY_AND_BUILD);
}
@@ -471,10 +493,12 @@ uint16_t smcp1GetMessageId()
return retVal;
}
-unsigned int smcp1CalculateMessageSize(const smcp1_frame *pFrame) {
+unsigned int smcp1CalculateMessageSize(const smcp1_frame *pFrame)
+{
unsigned int retVal = sizeof(smcp1_frame);
- for (int i = 0; i < pFrame->sub_blocks; i++) {
- smcp1_sb_header * pSbHeader = smcp1GetSubBlockHeader(pFrame, i);
+ for (int i = 0; i < pFrame->sub_blocks; i++)
+ {
+ smcp1_sb_header *pSbHeader = smcp1GetSubBlockHeader(pFrame, i);
retVal += sizeof(smcp1_sb_header) + (pSbHeader->size * smcp1SizeOfTypeBase(pSbHeader->type));
}
return retVal;
@@ -514,22 +538,22 @@ static mcu_error mcu_smcp10_resolve_target(const mcu_event *pEvent,
{
mcu_error retval = MCU_ERROR_NONE;
- switch(pEvent->frame.type)
+ switch (pEvent->frame.type)
{
- case SMCP1_CMD_STOP:
- case SMCP1_GET_VERSION:
- case SMCP1_GET_INFO_TEXT:
- case SMCP1_CMD_TAKE_STEP:
- case SMCP1_CMD_TAKE_LEGACY_STEP:
- case SMCP1_GET_PARAMETER:
- case SMCP1_SET_PARAMETER:
- case SMCP1_CMD_STORE_MEM:
- case SMCP1_CMD_GOTO_MEM:
- *dest = MCU_EVENT_QUEUE_COMM;
- break;
- default:
- retval = MCU_ERROR_NOT_SUPPORTED;
- break;
+ case SMCP1_CMD_STOP:
+ case SMCP1_GET_VERSION:
+ case SMCP1_GET_INFO_TEXT:
+ case SMCP1_CMD_TAKE_STEP:
+ case SMCP1_CMD_TAKE_LEGACY_STEP:
+ case SMCP1_GET_PARAMETER:
+ case SMCP1_SET_PARAMETER:
+ case SMCP1_CMD_STORE_MEM:
+ case SMCP1_CMD_GOTO_MEM:
+ *dest = MCU_EVENT_QUEUE_COMM;
+ break;
+ default:
+ retval = MCU_ERROR_NOT_SUPPORTED;
+ break;
}
return retval;
@@ -539,41 +563,42 @@ cmd_exec_handler SMCP10_ResolveCmdHandler(int cmd)
{
cmd_exec_handler retval;
- switch (cmd) {
- case SMCP1_CMD_STOP:
- retval = handle_smcp10_cmd_stop;
- break;
- case SMCP1_GET_PARAMETER:
- retval = handle_smcp10_cmd_get_parameter;
- break;
- case SMCP1_SET_PARAMETER:
- retval = handle_smcp10_cmd_set_parameter;
- break;
- case SMCP1_GET_VERSION:
- retval = handle_smcp10_cmd_get_version;
- break;
- case SMCP1_CMD_STORE_MEM:
- retval = handle_smcp10_cmd_store_mem;
- break;
- case SMCP1_CMD_GOTO_MEM:
- retval = handle_smcp10_cmd_goto_mem;
- break;
- case SMCP1_CMD_GOTO_MEM_REVERSE:
- retval = handle_smcp10_cmd_goto_mem_reverse;
- break;
- case SMCP1_GET_INFO_TEXT:
- retval = handle_smcp10_cmd_get_info_text;
- break;
- case SMCP1_CMD_TAKE_STEP:
- retval = handle_smcp10_cmd_take_step;
- break;
- case SMCP1_CMD_TAKE_LEGACY_STEP:
- retval = handle_smcp10_cmd_take_legacy_step;
- break;
-
- default:
- retval = NULL;
- break;
+ switch (cmd)
+ {
+ case SMCP1_CMD_STOP:
+ retval = handle_smcp10_cmd_stop;
+ break;
+ case SMCP1_GET_PARAMETER:
+ retval = handle_smcp10_cmd_get_parameter;
+ break;
+ case SMCP1_SET_PARAMETER:
+ retval = handle_smcp10_cmd_set_parameter;
+ break;
+ case SMCP1_GET_VERSION:
+ retval = handle_smcp10_cmd_get_version;
+ break;
+ case SMCP1_CMD_STORE_MEM:
+ retval = handle_smcp10_cmd_store_mem;
+ break;
+ case SMCP1_CMD_GOTO_MEM:
+ retval = handle_smcp10_cmd_goto_mem;
+ break;
+ case SMCP1_CMD_GOTO_MEM_REVERSE:
+ retval = handle_smcp10_cmd_goto_mem_reverse;
+ break;
+ case SMCP1_GET_INFO_TEXT:
+ retval = handle_smcp10_cmd_get_info_text;
+ break;
+ case SMCP1_CMD_TAKE_STEP:
+ retval = handle_smcp10_cmd_take_step;
+ break;
+ case SMCP1_CMD_TAKE_LEGACY_STEP:
+ retval = handle_smcp10_cmd_take_legacy_step;
+ break;
+
+ default:
+ retval = NULL;
+ break;
}
return retval;
}
@@ -598,7 +623,7 @@ static void initSMCP10_PostExecutionHandler(post_exec_handler *handler)
handler++;
*handler = &handle_smcp10_post_exec_opt_priority;
handler++;
- *handler = NULL; // SMCP1_OPT_WAIT_TRIGGER_1
+ *handler = NULL; // SMCP1_OPT_WAIT_TRIGGER_1
}
void mcu_smcp10SetLatestError(int32_t error)
@@ -613,7 +638,7 @@ void smcp10_execute_error_handler(mcu_event *pEvent, int error)
}
static void handle_smcp10_pre_exec_opt_wait_trigger_1(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -622,7 +647,7 @@ static void handle_smcp10_pre_exec_opt_wait_trigger_1(const mcu_event_queue targ
}
static void handle_smcp10_pre_exec_opt_priority(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -631,7 +656,7 @@ static void handle_smcp10_pre_exec_opt_priority(const mcu_event_queue target,
}
static void handle_smcp10_pre_exec_opt_req_bcast(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pStatus);
@@ -644,24 +669,24 @@ static void handle_smcp10_pre_exec_opt_req_bcast(const mcu_event_queue target,
}
static void handle_smcp10_pre_exec_opt_req_notify(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
UNUSED(pStatus);
-// MCUTRACE(("%s\n", __func__));
+ // MCUTRACE(("%s\n", __func__));
}
static void handle_smcp10_pre_exec_opt_req_resp(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
UNUSED(pStatus);
-// MCUTRACE(("%s\n", __func__));
+ // MCUTRACE(("%s\n", __func__));
}
static void handle_smcp10_pre_exec_opt_req_ack(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pStatus);
@@ -683,7 +708,7 @@ static void handle_smcp10_pre_exec_opt_req_ack(const mcu_event_queue target,
mcu_smcp10_hton_frame(&reply, &reply);
// Send the data
- mcu_error status = sendUdpData((const uint8_t*)&reply, sizeof(smcp1_frame),
+ mcu_error status = sendUdpData((const uint8_t *)&reply, sizeof(smcp1_frame),
(const struct sockaddr *)&pEvent->replyaddr);
assert(status == MCU_ERROR_NONE || status == MCU_ERROR_UDP_PEER_LOST);
@@ -691,7 +716,7 @@ static void handle_smcp10_pre_exec_opt_req_ack(const mcu_event_queue target,
}
static void handle_smcp10_pre_exec_opt_error(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -700,16 +725,16 @@ static void handle_smcp10_pre_exec_opt_error(const mcu_event_queue target,
}
static void handle_smcp10_pre_exec_opt_notify(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
UNUSED(pStatus);
-// MCUTRACE(("%s\n", __func__));
+ // MCUTRACE(("%s\n", __func__));
}
static void handle_smcp10_pre_exec_opt_ack(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -718,7 +743,7 @@ static void handle_smcp10_pre_exec_opt_ack(const mcu_event_queue target,
}
static void handle_smcp10_pre_exec_opt_req(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -727,7 +752,7 @@ static void handle_smcp10_pre_exec_opt_req(const mcu_event_queue target,
}
static void handle_smcp10_post_exec_opt_priority(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -736,7 +761,7 @@ static void handle_smcp10_post_exec_opt_priority(const mcu_event_queue target,
}
static void handle_smcp10_post_exec_opt_req_bcast(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -745,13 +770,13 @@ static void handle_smcp10_post_exec_opt_req_bcast(const mcu_event_queue target,
}
static void handle_smcp10_post_exec_opt_req_notify(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
if (mcu_executorIsEventDone(pStatus))
{
- int dataSize = sizeof(smcp1_frame)+sizeof(smcp1_sb_header)+sizeof(uint32_t);
+ int dataSize = sizeof(smcp1_frame) + sizeof(smcp1_sb_header) + sizeof(uint32_t);
char buffer[dataSize];
smcp1_frame *pReply = (smcp1_frame *)&buffer[0];
@@ -768,24 +793,25 @@ static void handle_smcp10_post_exec_opt_req_notify(const mcu_event_queue target,
// Resolve a notification type.
uint16_t type;
- switch(pEvent->rootType) {
-
- case SMCP1_CMD_INIT_ZERO:
- type = SMCP1_NOTIFY_GOTO_ZERO_COMPLETED;
- break;
- case SMCP1_CMD_GOTO_MEM:
- case SMCP1_CMD_GOTO_POS:
- type = SMCP1_NOTIFY_GOTO_POS_COMPLETED;
- break;
- case SMCP1_CMD_CALIBRATE:
- type = SMCP1_NOTIFY_CALIBRATE_COMPLETED;
- break;
- case SMCP1_CMD_DRIVE_LOOP:
- type = SMCP1_NOTIFY_DRIVE_LOOP_COMPLETED;
- break;
- default:
- type = pEvent->rootType;
- break;
+ switch (pEvent->rootType)
+ {
+
+ case SMCP1_CMD_INIT_ZERO:
+ type = SMCP1_NOTIFY_GOTO_ZERO_COMPLETED;
+ break;
+ case SMCP1_CMD_GOTO_MEM:
+ case SMCP1_CMD_GOTO_POS:
+ type = SMCP1_NOTIFY_GOTO_POS_COMPLETED;
+ break;
+ case SMCP1_CMD_CALIBRATE:
+ type = SMCP1_NOTIFY_CALIBRATE_COMPLETED;
+ break;
+ case SMCP1_CMD_DRIVE_LOOP:
+ type = SMCP1_NOTIFY_DRIVE_LOOP_COMPLETED;
+ break;
+ default:
+ type = pEvent->rootType;
+ break;
}
// Set the resolved type.
@@ -797,58 +823,64 @@ static void handle_smcp10_post_exec_opt_req_notify(const mcu_event_queue target,
// One sub block must be included
// ... sb header
pReply->sub_blocks = 1;
- smcp1_sb_header *pSbHdr = (smcp1_sb_header *)((void*)pReply+sizeof(smcp1_frame));
+ smcp1_sb_header *pSbHdr = (smcp1_sb_header *)((void *)pReply + sizeof(smcp1_frame));
pSbHdr->size = 1;
pSbHdr->type = SMCP1_DATA_UINT32;
// ... sb data
- uint32_t *pData = (uint32_t *)((void*)pSbHdr+sizeof(smcp1_sb_header));
+ uint32_t *pData = (uint32_t *)((void *)pSbHdr + sizeof(smcp1_sb_header));
*pData = pStatus->error;
// Change the data format to network format (big-endian)
mcu_smcp10_hton_frame(pReply, pReply);
// Send the response data
- mcu_error status = sendUdpData((const uint8_t*)pReply, dataSize,
+ mcu_error status = sendUdpData((const uint8_t *)pReply, dataSize,
(const struct sockaddr *)&pEvent->replyaddr);
assert(status == MCU_ERROR_NONE || status == MCU_ERROR_UDP_PEER_LOST);
UNUSED(status);
-
}
}
static void handle_smcp10_post_exec_opt_req_resp(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
- if (pEvent->frame.options & SMCP1_OPT_REQ) {
+ if (pEvent->frame.options & SMCP1_OPT_REQ)
+ {
mcu_error status = MCU_ERROR_NONE;
- if (mcu_executorIsEventDone(pStatus)) {
+ if (mcu_executorIsEventDone(pStatus))
+ {
char *pRespData = NULL;
int respLen = 0;
smcp1_frame defaultreply;
- if (pStatus->pDataDeferred) {
+ if (pStatus->pDataDeferred)
+ {
assert(pEvent->pData1 == NULL);
pEvent->pData1 = (char *)pStatus->pDataDeferred;
// Ownership transferred
status = mcu_executorStatusInsertPayload(pStatus, NULL);
}
- if (status == MCU_ERROR_NONE) {
- if(pEvent->pData1) {
+ if (status == MCU_ERROR_NONE)
+ {
+ if (pEvent->pData1)
+ {
// We have a custom message for reply. Use it.
// The first 32 bit indicates the length of actual data
int *ptrExtData = (int *)(pEvent->pData1);
respLen = *ptrExtData;
ptrExtData++;
// ... the actual data starts after length block
- pRespData = (char*)ptrExtData;
- } else {
+ pRespData = (char *)ptrExtData;
+ }
+ else
+ {
// Default reply handler
- smcp1_frame * origFrame = getSMCPv1Frame(pEvent, &defaultreply);
+ smcp1_frame *origFrame = getSMCPv1Frame(pEvent, &defaultreply);
assert(origFrame);
// Switch the content of sender id and received id fields
@@ -866,24 +898,28 @@ static void handle_smcp10_post_exec_opt_req_resp(const mcu_event_queue target,
// Modify frame options
// ... No ack needed + this is a response (switch off a req flag).
- defaultreply.options &= (~(SMCP1_OPT_REQ_ACK|SMCP1_OPT_REQ|SMCP1_OPT_REQ_RESP|SMCP1_OPT_REQ_NOTIFY));
+ defaultreply.options &= (~(SMCP1_OPT_REQ_ACK | SMCP1_OPT_REQ | SMCP1_OPT_REQ_RESP | SMCP1_OPT_REQ_NOTIFY));
// Change the data format to network format (big-endian)
mcu_smcp10_hton_frame(&defaultreply, &defaultreply);
// Set send pointers
- pRespData = (char*)&defaultreply;
+ pRespData = (char *)&defaultreply;
respLen = sizeof(smcp1_frame);
}
// Send the response data
status = sendUdpData((const uint8_t *)pRespData, respLen, (const struct sockaddr *)&pEvent->replyaddr);
}
- } else {
- if (pEvent->pData1) {
+ }
+ else
+ {
+ if (pEvent->pData1)
+ {
// Save the payload
mcu_error status = mcu_executorStatusInsertPayload(pEvent->pStatus, pEvent->pData1);
- if (status == MCU_ERROR_NONE) {
+ if (status == MCU_ERROR_NONE)
+ {
// Ownership transferred
pEvent->pData1 = NULL;
}
@@ -894,16 +930,16 @@ static void handle_smcp10_post_exec_opt_req_resp(const mcu_event_queue target,
}
static void handle_smcp10_post_exec_opt_req_ack(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
UNUSED(pStatus);
-// MCUTRACE(("%s\n", __func__));
+ // MCUTRACE(("%s\n", __func__));
}
static void handle_smcp10_post_exec_opt_error(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -912,16 +948,16 @@ static void handle_smcp10_post_exec_opt_error(const mcu_event_queue target,
}
static void handle_smcp10_post_exec_opt_notify(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
UNUSED(pStatus);
-// MCUTRACE(("%s\n", __func__));
+ // MCUTRACE(("%s\n", __func__));
}
static void handle_smcp10_post_exec_opt_ack(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -930,7 +966,7 @@ static void handle_smcp10_post_exec_opt_ack(const mcu_event_queue target,
}
static void handle_smcp10_post_exec_opt_req(const mcu_event_queue target,
- mcu_event* pEvent, mcu_event_status *pStatus)
+ mcu_event *pEvent, mcu_event_status *pStatus)
{
UNUSED(target);
UNUSED(pEvent);
@@ -951,11 +987,12 @@ void SMCP10_RegisterExecutor(void *info)
info2->postCmdHandlers = smcp10_event_post_exec_options_handlers;
}
-smcp1_frame * getSMCPv1Frame(const mcu_event *pEvent, smcp1_frame *pTarget)
+smcp1_frame *getSMCPv1Frame(const mcu_event *pEvent, smcp1_frame *pTarget)
{
- smcp1_frame *pSource = (smcp1_frame*)smcpv10_find_element(pEvent, SCMP1_FRAME_ELEM);
+ smcp1_frame *pSource = (smcp1_frame *)smcpv10_find_element(pEvent, SCMP1_FRAME_ELEM);
- if (pTarget) {
+ if (pTarget)
+ {
// Make a copy of the original frame
memcpy(pTarget, pSource, sizeof(smcp1_frame));
}
@@ -963,27 +1000,28 @@ smcp1_frame * getSMCPv1Frame(const mcu_event *pEvent, smcp1_frame *pTarget)
}
char *smcpv10_compose_reply(mcu_event *pReqEvent, char *pData, int size,
- smcp1_sb_data_type type)
+ smcp1_sb_data_type type)
{
int typeSize = mcu_smcp10SizeofTypeBase(type);
- int respSize = (int)(sizeof(uint32_t)+sizeof(smcp1_frame)+
- sizeof(smcp1_sb_header)+(size*typeSize));
+ int respSize = (int)(sizeof(uint32_t) + sizeof(smcp1_frame) +
+ sizeof(smcp1_sb_header) + (size * typeSize));
char *retval = mcuMalloc(respSize);
- if(retval) {
+ if (retval)
+ {
// compose a message
// Data size is the very first element of extension data block
- uint32_t *tmp = (uint32_t*)retval;
- *tmp = respSize-sizeof(uint32_t); // The size of actual data element
+ uint32_t *tmp = (uint32_t *)retval;
+ *tmp = respSize - sizeof(uint32_t); // The size of actual data element
tmp++;
// The actual data goes right after the size
- smcp1_frame* pResp = (smcp1_frame*)tmp;
- smcp1_frame *pReq = (smcp1_frame *) smcpv10_find_element(pReqEvent, SCMP1_FRAME_ELEM);
+ smcp1_frame *pResp = (smcp1_frame *)tmp;
+ smcp1_frame *pReq = (smcp1_frame *)smcpv10_find_element(pReqEvent, SCMP1_FRAME_ELEM);
- smcp1_sb_header *pRespSb = (smcp1_sb_header *)((void*)pResp + sizeof(smcp1_frame));
+ smcp1_sb_header *pRespSb = (smcp1_sb_header *)((void *)pResp + sizeof(smcp1_frame));
// Make a raw copy
memcpy(pResp, pReq, sizeof(smcp1_frame));
@@ -1003,8 +1041,8 @@ char *smcpv10_compose_reply(mcu_event *pReqEvent, char *pData, int size,
pRespSb->type = type;
// ... sb data
- char *pRespSbData = (char*)((void*)pRespSb + sizeof(smcp1_sb_header));
- memcpy(pRespSbData, pData, typeSize*size);
+ char *pRespSbData = (char *)((void *)pRespSb + sizeof(smcp1_sb_header));
+ memcpy(pRespSbData, pData, typeSize * size);
// Finally. Convert the data from host to network format
mcu_smcp10_hton_frame(pResp, pResp);
@@ -1012,30 +1050,36 @@ char *smcpv10_compose_reply(mcu_event *pReqEvent, char *pData, int size,
return retval;
}
-static void mcu_smcpv10_copyPeer(mcu_smcp10_peer * dest, const mcu_smcp10_peer * src)
+static void mcu_smcpv10_copyPeer(mcu_smcp10_peer *dest, const mcu_smcp10_peer *src)
{
pthread_mutex_lock(&peerMutex);
memcpy(dest, src, sizeof(mcu_smcp10_peer));
pthread_mutex_unlock(&peerMutex);
}
-static mcu_error mcu_smcpv10_getPeer(const uint32_t index, mcu_smcp10_peer * peer)
+static mcu_error mcu_smcpv10_getPeer(const uint32_t index, mcu_smcp10_peer *peer)
{
mcu_error retVal = MCU_ERROR_NONE;
- if (index < SMCP10_PEER_ADDR_CACHE_SIZE) {
+ if (index < SMCP10_PEER_ADDR_CACHE_SIZE)
+ {
mcu_smcpv10_copyPeer(peer, &g_peer_address_cache[index]);
- } else {
+ }
+ else
+ {
retVal = MCU_ERROR_OVERFLOW;
}
return retVal;
}
-static mcu_error mcu_smcpv10_setPeer(mcu_smcp10_peer * peer, const uint32_t index)
+static mcu_error mcu_smcpv10_setPeer(mcu_smcp10_peer *peer, const uint32_t index)
{
mcu_error retVal = MCU_ERROR_NONE;
- if (index < SMCP10_PEER_ADDR_CACHE_SIZE) {
+ if (index < SMCP10_PEER_ADDR_CACHE_SIZE)
+ {
mcu_smcpv10_copyPeer(&g_peer_address_cache[index], peer);
- } else {
+ }
+ else
+ {
retVal = MCU_ERROR_OVERFLOW;
}
return retVal;
@@ -1050,32 +1094,37 @@ static void mcu_smcp10_updatePeerAddressCache(const in_addr_t *addr, uint16_t po
bool updated = false;
mcu_smcp10_peer tmp_peer;
- mcu_smcp10_peer * peer = &tmp_peer;
+ mcu_smcp10_peer *peer = &tmp_peer;
- for (uint32_t i = 0; i < SMCP10_PEER_ADDR_CACHE_SIZE; i++) {
+ for (uint32_t i = 0; i < SMCP10_PEER_ADDR_CACHE_SIZE; i++)
+ {
// Detect the oldest peer address, it will be overwritten
// with a new one if all address slots are in use.
// This will also detect the first empty slot
mcu_smcpv10_getPeer(i, peer);
- if(peer->ts < oldest_ts) {
+ if (peer->ts < oldest_ts)
+ {
oldest_ts = peer->ts;
entry_index = i;
}
// An empty slot
- if (!peer->ts) {
+ if (!peer->ts)
+ {
continue;
}
// Timer wraparound handling
- if (peer->ts > ts) {
- peer->ts = ts;
+ if (peer->ts > ts)
+ {
+ peer->ts = ts;
}
// Update the existing entry in cache
- if (peer->addr == *addr && peer->port == port) {
+ if (peer->addr == *addr && peer->port == port)
+ {
peer->ts = ts;
entry_index = i;
updated = true;
@@ -1084,7 +1133,8 @@ static void mcu_smcp10_updatePeerAddressCache(const in_addr_t *addr, uint16_t po
}
// Add new entry in cache
- if (!updated) {
+ if (!updated)
+ {
peer->ts = ts;
peer->port = port;
peer->addr = *addr;
@@ -1099,22 +1149,24 @@ mcu_error mcu_smcp10_sendToAll(const char *data, unsigned int datasize)
mcu_error status, retval = MCU_ERROR_NONE;
mcu_smcp10_peer tmp_peer;
- mcu_smcp10_peer * peer = &tmp_peer;
+ mcu_smcp10_peer *peer = &tmp_peer;
struct sockaddr_in addr;
memset(&addr, 0, sizeof(addr));
addr.sin_family = AF_INET; // IPv4
- for(uint32_t i = 0; i < SMCP10_PEER_ADDR_CACHE_SIZE; i++) {
+ for (uint32_t i = 0; i < SMCP10_PEER_ADDR_CACHE_SIZE; i++)
+ {
mcu_smcpv10_getPeer(i, peer);
- if(!peer->ts)
+ if (!peer->ts)
continue;
// Check if peer is lost by not got any message from it for a long time.
- if(mcu_commonIsExpired(HAL_GetTick(), peer->ts,
- SMCP10_PEER_ADDR_CACHE_TIMEOUT_MS)) {
+ if (mcu_commonIsExpired(HAL_GetTick(), peer->ts,
+ SMCP10_PEER_ADDR_CACHE_TIMEOUT_MS))
+ {
memset(peer, 0, sizeof(mcu_smcp10_peer));
mcu_smcpv10_setPeer(peer, i);
continue;
@@ -1122,47 +1174,53 @@ mcu_error mcu_smcp10_sendToAll(const char *data, unsigned int datasize)
addr.sin_addr.s_addr = peer->addr;
addr.sin_port = peer->port;
- sendUdpMessage((const smcp1_frame *)data, (struct sockaddr *) &addr);
+ sendUdpMessage((const smcp1_frame *)data, (struct sockaddr *)&addr);
// Clear address if peer is lost
- if(status == MCU_ERROR_UDP_PEER_LOST) {
+ if (status == MCU_ERROR_UDP_PEER_LOST)
+ {
memset(peer, 0, sizeof(mcu_smcp10_peer));
mcu_smcpv10_setPeer(peer, i);
}
- if(status != MCU_ERROR_NONE)
+ if (status != MCU_ERROR_NONE)
retval = status;
}
return retval;
}
-uint16_t smcp10_deviceID() {
+uint16_t smcp10_deviceID()
+{
int retval;
mcu_error status = mcu_smcp10_read_param(SMCP1_PARAM_DEV_ID, &retval, false);
- if (status == MCU_ERROR_NONE) {
+ if (status == MCU_ERROR_NONE)
+ {
return (uint16_t)retval;
- } else {
+ }
+ else
+ {
return 0;
}
return retval;
}
-char* smcpv10_find_element(const mcu_event *pEvent,
- smcp1_mcu_event_element block)
+char *smcpv10_find_element(const mcu_event *pEvent,
+ smcp1_mcu_event_element block)
{
- char* retval = (char*)pEvent;
+ char *retval = (char *)pEvent;
- switch(block) {
- case SCMP1_MCU_EVENT_ELEM:
- break;
- case SCMP1_FRAME_ELEM:
- retval = (char*)&pEvent->frame;
- break;
- case SCMP1_SUBBLOCK_ELEM:
- retval = (char *)pEvent + sizeof(mcu_event);
- break;
- default:
- assert(false); // Unknown parameter. Assert it.
+ switch (block)
+ {
+ case SCMP1_MCU_EVENT_ELEM:
+ break;
+ case SCMP1_FRAME_ELEM:
+ retval = (char *)&pEvent->frame;
+ break;
+ case SCMP1_SUBBLOCK_ELEM:
+ retval = (char *)pEvent + sizeof(mcu_event);
+ break;
+ default:
+ assert(false); // Unknown parameter. Assert it.
}
return retval;
}
@@ -1171,48 +1229,48 @@ int mcu_smcp10SizeofTypeBase(smcp1_sb_data_type datatype)
{
int retval;
- switch(datatype)
+ switch (datatype)
{
- case SMCP1_DATA_VOID:
- retval = sizeof(void);
- break;
- case SMCP1_DATA_UINT8:
- case SMCP1_DATA_UINT8_ARRAY_2:
- case SMCP1_DATA_UINT8_ARRAY_3:
- case SMCP1_DATA_UINT8_ARRAY_4:
- case SMCP1_DATA_CHAR_STRING:
- retval = sizeof(uint8_t);
- break;
- case SMCP1_DATA_INT8:
- case SMCP1_DATA_INT8_ARRAY_2:
- case SMCP1_DATA_INT8_ARRAY_3:
- case SMCP1_DATA_INT8_ARRAY_4:
- retval = sizeof(int8_t);
- break;
- case SMCP1_DATA_UINT16:
- case SMCP1_DATA_UINT16_ARRAY_2:
- retval = sizeof(uint16_t);
- break;
- case SMCP1_DATA_INT16:
- case SMCP1_DATA_INT16_ARRAY_2:
- retval = sizeof(int16_t);
- break;
- case SMCP1_DATA_UINT32:
- retval = sizeof(uint32_t);
- break;
- case SMCP1_DATA_INT32:
- retval = sizeof(int32_t);
- break;
- case SMCP1_DATA_UINT64:
- retval = sizeof(uint64_t);
- break;
- case SMCP1_DATA_INT64:
- retval = sizeof(int64_t);
- break;
- default:
- retval = 0;
- assert(false); //Unknown datatype. Assert it.
- break;
+ case SMCP1_DATA_VOID:
+ retval = sizeof(void);
+ break;
+ case SMCP1_DATA_UINT8:
+ case SMCP1_DATA_UINT8_ARRAY_2:
+ case SMCP1_DATA_UINT8_ARRAY_3:
+ case SMCP1_DATA_UINT8_ARRAY_4:
+ case SMCP1_DATA_CHAR_STRING:
+ retval = sizeof(uint8_t);
+ break;
+ case SMCP1_DATA_INT8:
+ case SMCP1_DATA_INT8_ARRAY_2:
+ case SMCP1_DATA_INT8_ARRAY_3:
+ case SMCP1_DATA_INT8_ARRAY_4:
+ retval = sizeof(int8_t);
+ break;
+ case SMCP1_DATA_UINT16:
+ case SMCP1_DATA_UINT16_ARRAY_2:
+ retval = sizeof(uint16_t);
+ break;
+ case SMCP1_DATA_INT16:
+ case SMCP1_DATA_INT16_ARRAY_2:
+ retval = sizeof(int16_t);
+ break;
+ case SMCP1_DATA_UINT32:
+ retval = sizeof(uint32_t);
+ break;
+ case SMCP1_DATA_INT32:
+ retval = sizeof(int32_t);
+ break;
+ case SMCP1_DATA_UINT64:
+ retval = sizeof(uint64_t);
+ break;
+ case SMCP1_DATA_INT64:
+ retval = sizeof(int64_t);
+ break;
+ default:
+ retval = 0;
+ assert(false); // Unknown datatype. Assert it.
+ break;
}
return retval;
}
@@ -1240,7 +1298,8 @@ static bool mcu_smcp10_accept(const smcp1_frame *pFrame)
if (pFrame->receiver_id == ownId ||
(pFrame->receiver_id == SMCP1_ALL_DEVICES &&
- pFrame->sender_id != ownId)) {
+ pFrame->sender_id != ownId))
+ {
retval = true;
}
@@ -1255,21 +1314,23 @@ mcu_error mcu_smcp10_handleRawEvent(char *rawData, int rawDataLen, struct sockad
// mcu_event has a few custom fields also.
// Allocate a single heap block for all this stuff.
int rawEventSize = rawDataLen;
- int eventSize = rawEventSize + (int)(sizeof(mcu_event) + (sizeof(smcp1_sb_header) * 2)); // An estimation
+ int eventSize = rawEventSize + (int)(sizeof(mcu_event) + (sizeof(smcp1_sb_header) * 2)); // An estimation
mcu_event *pEvent = mcuMalloc(eventSize);
- if (pEvent) {
+ if (pEvent)
+ {
mcu_event_queue dest;
// Resolve a pointer location for smcpv1 frame ...
- smcp1_frame *pSmcpv1_frame = (smcp1_frame*) smcpv10_find_element(pEvent, SCMP1_FRAME_ELEM);
+ smcp1_frame *pSmcpv1_frame = (smcp1_frame *)smcpv10_find_element(pEvent, SCMP1_FRAME_ELEM);
// ... Convert the event format to host format
- mcu_smcp10_ntoh_frame(pSmcpv1_frame, (smcp1_frame*)rawData);
+ mcu_smcp10_ntoh_frame(pSmcpv1_frame, (smcp1_frame *)rawData);
// ... Check the recipient
- if (mcu_smcp10_accept(pSmcpv1_frame)) {
+ if (mcu_smcp10_accept(pSmcpv1_frame))
+ {
// ... Add the content of pure mcu_event specific fields
memcpy(&pEvent->replyaddr, recv_addr, sizeof(struct sockaddr));
@@ -1286,25 +1347,28 @@ mcu_error mcu_smcp10_handleRawEvent(char *rawData, int rawDataLen, struct sockad
// ... Resolve a target queue.
retval = mcu_smcp10_resolve_target(pEvent, &dest);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
// This is the root event. Set the type.
pEvent->rootType = pEvent->frame.type;
// Send an ACK immediately
- if(pEvent->frame.options & SMCP1_OPT_REQ_ACK) {
+ if (pEvent->frame.options & SMCP1_OPT_REQ_ACK)
+ {
handle_smcp10_pre_exec_opt_req_ack(dest, pEvent, NULL);
}
const struct sockaddr_in *r_addr = (struct sockaddr_in *)recv_addr;
- const in_addr_t *addr = (in_addr_t*)&r_addr->sin_addr;
+ const in_addr_t *addr = (in_addr_t *)&r_addr->sin_addr;
const uint16_t port = (uint16_t)r_addr->sin_port;
mcu_smcp10_updatePeerAddressCache(addr, port);
// Append the event in target executor queue
retval = mcu_executorInsertEvent(dest, pEvent);
}
- } else
+ }
+ else
retval = MCU_ERROR_NO_ACCESS;
// The last step. Release the allocated resources.
@@ -1317,59 +1381,62 @@ mcu_error mcu_smcp10_read_param(uint32_t paramId, int *value, bool refresh_cache
{
mcu_error retval = MCU_ERROR_OVERFLOW;
- if (paramId < SMCP10_PARAM_COUNT) {
+ if (paramId < SMCP10_PARAM_COUNT)
+ {
retval = MCU_ERROR_NONE;
- if (refresh_cache) {
- //TODO
+ if (refresh_cache)
+ {
+ // TODO
}
if (retval == MCU_ERROR_NONE)
*value = smcp10_params_cache[paramId];
-
-
- } else if (paramId >= SMCP10_PARAM_DYNAMIC_START &&
- paramId <= SMCP10_PARAM_DYNAMIC_END) {
+ }
+ else if (paramId >= SMCP10_PARAM_DYNAMIC_START &&
+ paramId <= SMCP10_PARAM_DYNAMIC_END)
+ {
retval = MCU_ERROR_NONE;
switch (paramId)
{
- default:
- retval = MCU_ERROR_NOT_FOUND;
- break;
+ default:
+ retval = MCU_ERROR_NOT_FOUND;
+ break;
}
-
- } else if (paramId > SMCP10_PARAM_READONLY_START &&
- paramId <= SMCP10_PARAM_READONLY_END) {
+ }
+ else if (paramId > SMCP10_PARAM_READONLY_START &&
+ paramId <= SMCP10_PARAM_READONLY_END)
+ {
double angle;
retval = MCU_ERROR_NONE;
switch (paramId)
{
- case SMCP1_PARAM_HW_ID:
- case SMCP1_PARAM_SN:
- case SMCP1_PARAM_EOW:
- {
- const hwInfo *pHwInfo = mcu_commonGetHwInfo(false);
- if (paramId == SMCP1_PARAM_HW_ID)
- *value = (int)pHwInfo->hwid;
- else if (paramId == SMCP1_PARAM_SN)
- *value = (int)pHwInfo->serialNbr;
- else if (paramId == SMCP1_PARAM_EOW)
- *value = (int)pHwInfo->manufactured;
- else
- retval = MCU_ERROR_PARAMETER;
- break;
- }
- case SMCP1_PARAM_AXIS_COUNT:
- {
- systemInfo * pSystemInfo = mcu_commonGetSystemConf(false);
- int tmpValue = (int)mcu_commonResolveActuatorAxisCount(pSystemInfo);
- *value = tmpValue;
- break;
- }
- default:
- retval = MCU_ERROR_OVERFLOW;
- break;
+ case SMCP1_PARAM_HW_ID:
+ case SMCP1_PARAM_SN:
+ case SMCP1_PARAM_EOW:
+ {
+ const hwInfo *pHwInfo = mcu_commonGetHwInfo(false);
+ if (paramId == SMCP1_PARAM_HW_ID)
+ *value = (int)pHwInfo->hwid;
+ else if (paramId == SMCP1_PARAM_SN)
+ *value = (int)pHwInfo->serialNbr;
+ else if (paramId == SMCP1_PARAM_EOW)
+ *value = (int)pHwInfo->manufactured;
+ else
+ retval = MCU_ERROR_PARAMETER;
+ break;
+ }
+ case SMCP1_PARAM_AXIS_COUNT:
+ {
+ systemInfo *pSystemInfo = mcu_commonGetSystemConf(false);
+ int tmpValue = (int)mcu_commonResolveActuatorAxisCount(pSystemInfo);
+ *value = tmpValue;
+ break;
+ }
+ default:
+ retval = MCU_ERROR_OVERFLOW;
+ break;
}
}
return retval;
@@ -1378,25 +1445,29 @@ mcu_error mcu_smcp10_read_param(uint32_t paramId, int *value, bool refresh_cache
mcu_error mcu_smcp10_write_param(uint32_t paramID, int value, bool write_through)
{
mcu_error retval = MCU_ERROR_NO_ACCESS;
- if (paramID < SMCP10_PARAM_COUNT) {
+ if (paramID < SMCP10_PARAM_COUNT)
+ {
retval = MCU_ERROR_NONE;
- if (write_through) {
+ if (write_through)
+ {
// TODO
}
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
smcp10_params_cache[paramID] = value;
}
-
}
return retval;
}
-uint32_t mcu_commonResolveActuatorAxisCount(const systemInfo * info)
+uint32_t mcu_commonResolveActuatorAxisCount(const systemInfo *info)
{
uint32_t retVal = 0;
- const systemInfo * sysInfoPtr = info ? info : mcu_commonGetSystemConf(false);
- for (int act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++) {
- if (mcu_commonIsActuatorEnabled(act, sysInfoPtr)) {
+ const systemInfo *sysInfoPtr = info ? info : mcu_commonGetSystemConf(false);
+ for (int act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++)
+ {
+ if (mcu_commonIsActuatorEnabled(act, sysInfoPtr))
+ {
retVal++;
}
}
@@ -1410,38 +1481,44 @@ static mcu_error mcu_smcp10_set_default_params()
mcu_error retval;
- for (param = 0; param < SMCP10_PARAM_COUNT; param++) {
+ for (param = 0; param < SMCP10_PARAM_COUNT; param++)
+ {
retval = mcu_smcp10_read_param(param, &value, false);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
- if (value == SMCP10_PARAM_UNINITIALIZED) {
+ if (value == SMCP10_PARAM_UNINITIALIZED)
+ {
- switch(param)
+ switch (param)
{
- case SMCP1_PARAM_MEM_SPEED:
- value = 4;
- break;
- case SMCP1_PARAM_DEV_ID:
+ case SMCP1_PARAM_MEM_SPEED:
+ value = 4;
+ break;
+ case SMCP1_PARAM_DEV_ID:
+ {
+ int tmcDevId = integrationGetDev();
+ if (tmcDevId == 0)
{
- int tmcDevId = integrationGetDev();
- if (tmcDevId == 0) {
- value = 1;
- integrationSetDev(value);
- } else {
- value = tmcDevId;
- }
- break;
+ value = 1;
+ integrationSetDev(value);
}
- case SMCP1_PARAM_AXIS_DRIVE_ORDER:
- value = ACTUATOR_0;
- value |= (ACTUATOR_2 << 8);
- value |= (ACTUATOR_1 << 16);
- value |= (ACTUATOR_3 << 24);
- break;
- default:
- break;
+ else
+ {
+ value = tmcDevId;
+ }
+ break;
+ }
+ case SMCP1_PARAM_AXIS_DRIVE_ORDER:
+ value = ACTUATOR_0;
+ value |= (ACTUATOR_2 << 8);
+ value |= (ACTUATOR_1 << 16);
+ value |= (ACTUATOR_3 << 24);
+ break;
+ default:
+ break;
}
// Update the cache when needed
@@ -1450,7 +1527,8 @@ static mcu_error mcu_smcp10_set_default_params()
}
}
- if (retval != MCU_ERROR_NONE) {
+ if (retval != MCU_ERROR_NONE)
+ {
break;
}
}
@@ -1463,22 +1541,23 @@ static mcu_error smcp10_set_param_refresh_system(uint32_t paramId, int value)
switch (paramId)
{
- case SMCP1_PARAM_UDP_PORT:
+ case SMCP1_PARAM_UDP_PORT:
+ {
+ systemInfo *pSysInfo = mcu_commonGetSystemConf(false);
+ pSysInfo->udp_port = (value & 0xffff);
+ retval = mcu_commonSetSystemConf(pSysInfo, true);
+ if (retval == MCU_ERROR_NONE)
{
- systemInfo *pSysInfo = mcu_commonGetSystemConf(false);
- pSysInfo->udp_port = (value & 0xffff);
- retval = mcu_commonSetSystemConf(pSysInfo, true);
- if (retval == MCU_ERROR_NONE) {
- // TODO re-init socket connection with new port
- }
- break;
+ // TODO re-init socket connection with new port
}
- case SMCP1_PARAM_DEV_ID:
- integrationSetDev(value);
- break;
+ break;
+ }
+ case SMCP1_PARAM_DEV_ID:
+ integrationSetDev(value);
+ break;
- default:
- break;
+ default:
+ break;
}
return retval;
}
@@ -1494,34 +1573,42 @@ static mcu_error smcp10_resolve_mem_drive_order(uint32_t mem_index, bool *result
mcu_error retval = mcu_smcp10_read_param(SMCP1_PARAM_AXIS_DRIVE_ORDER,
(int *)&driveOrder, false);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
systemInfo *info = mcu_commonGetSystemConf(false);
- if (mcu_commonResolveActuatorAxisCount(info) > 0) {
+ if (mcu_commonResolveActuatorAxisCount(info) > 0)
+ {
uint32_t i;
uint8_t *pAct = (uint8_t *)&driveOrder;
bool found = false;
- for (i = 0; i < sizeof driveOrder; i++) {
- if (mcu_commonIsActuatorEnabled(*pAct, info)) {
+ for (i = 0; i < sizeof driveOrder; i++)
+ {
+ if (mcu_commonIsActuatorEnabled(*pAct, info))
+ {
found = true;
break;
}
pAct++;
}
- if (found) {
+ if (found)
+ {
// Resolve current and target positions
int currentPos;
int32_t targetPos;
uint8_t tmp = *pAct;
mcu_actuator act = (mcu_actuator)tmp;
retval = mcu_actuatorGetPosition(act, ¤tPos, false);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
retval = mcu_smcp10_read_mem_drive_target(act, mem_index, &targetPos);
// Set the actual result
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
bool reverse = true;
- if (currentPos > (int)targetPos) {
+ if (currentPos > (int)targetPos)
+ {
reverse = false;
}
@@ -1537,30 +1624,34 @@ static mcu_error smcp10_resolve_mem_drive_order(uint32_t mem_index, bool *result
/**************** The SCMP1 command handlers ****************/
-int handle_smcp10_cmd_get_parameter(mcu_event_queue target, mcu_event *pEvent) {
+int handle_smcp10_cmd_get_parameter(mcu_event_queue target, mcu_event *pEvent)
+{
UNUSED(target);
char *pSb_data;
smcp1_sb_header *pSb;
mcu_error retval = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSb, &pSb_data);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
retval = MCU_ERROR_PARAMETER;
- if (pSb->size >= 1) {
+ if (pSb->size >= 1)
+ {
- uint32_t paramId = *(uint32_t *) smcpv10_get_data(pSb, pSb_data, 0);
+ uint32_t paramId = *(uint32_t *)smcpv10_get_data(pSb, pSb_data, 0);
int value = -1;
retval = mcu_smcp10_read_param(paramId, &value, false);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
int32_t replyData[2];
replyData[0] = (int32_t)paramId;
replyData[1] = value;
- char *reply = smcpv10_compose_reply(pEvent, (char *) &replyData[0], 2, SMCP1_DATA_INT32);
+ char *reply = smcpv10_compose_reply(pEvent, (char *)&replyData[0], 2, SMCP1_DATA_INT32);
if (reply)
pEvent->pData1 = reply;
@@ -1582,31 +1673,38 @@ int handle_smcp10_cmd_set_parameter(const mcu_event_queue target, mcu_event *pEv
smcp1_sb_header *pSb;
retval = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSb, &pSb_data);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
- if (pSb->size >= 2) {
+ if (pSb->size >= 2)
+ {
- uint32_t paramId = *(uint32_t*)smcpv10_get_data(pSb, pSb_data, 0);
- int value = *(int*)smcpv10_get_data(pSb, pSb_data, 1);
+ uint32_t paramId = *(uint32_t *)smcpv10_get_data(pSb, pSb_data, 0);
+ int value = *(int *)smcpv10_get_data(pSb, pSb_data, 1);
// Write normal static data in EEPROM
- if (paramId < SMCP10_PARAM_DYNAMIC_START) {
+ if (paramId < SMCP10_PARAM_DYNAMIC_START)
+ {
retval = mcu_smcp10_write_param(paramId, value, true);
}
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
// Refresh
retval = smcp10_set_param_refresh_system(paramId, value);
}
MCUTRACE(("%s param = %d, value = %d\n", __func__, (int)paramId, value));
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_PARAMETER;
}
}
- return retval;
+ return retval;
}
-int handle_smcp10_cmd_get_version(mcu_event_queue target, mcu_event *pEvent) {
+int handle_smcp10_cmd_get_version(mcu_event_queue target, mcu_event *pEvent)
+{
UNUSED(target);
mcu_error retval = MCU_ERROR_NONE;
@@ -1616,7 +1714,7 @@ int handle_smcp10_cmd_get_version(mcu_event_queue target, mcu_event *pEvent) {
replyData[2] = SMCP_PROXY_VERSION_WEEK;
replyData[3] = SMCP_PROXY_VERSION_DAY_AND_BUILD;
- char *reply = smcpv10_compose_reply(pEvent, (char *) &replyData[0],4, SMCP1_DATA_UINT32);
+ char *reply = smcpv10_compose_reply(pEvent, (char *)&replyData[0], 4, SMCP1_DATA_UINT32);
if (reply)
pEvent->pData1 = reply;
@@ -1633,18 +1731,21 @@ int handle_smcp10_cmd_get_info_text(mcu_event_queue target, mcu_event *pEvent)
char buffer[120];
char *pCtrlFirmwareVersion = integrationGetFirmwareVersion();
sprintf(buffer, "Primary: SPXY v%d.%02d.%02d.%03d\nSecondary: %s", SMCP_PROXY_VERSION_MAJOR,
- SMCP_PROXY_VERSION_YEAR, SMCP_PROXY_VERSION_WEEK, SMCP_PROXY_VERSION_DAY_AND_BUILD,
- pCtrlFirmwareVersion);
+ SMCP_PROXY_VERSION_YEAR, SMCP_PROXY_VERSION_WEEK, SMCP_PROXY_VERSION_DAY_AND_BUILD,
+ pCtrlFirmwareVersion);
mcu_error retval = MCU_ERROR_NONE;
int payloadSize = (int)strlen(buffer);
- char *reply = smcpv10_compose_reply(pEvent, (char*)&buffer[0],payloadSize, SMCP1_DATA_CHAR_STRING);
+ char *reply = smcpv10_compose_reply(pEvent, (char *)&buffer[0], payloadSize, SMCP1_DATA_CHAR_STRING);
- if (reply) {
+ if (reply)
+ {
pEvent->pData1 = reply;
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_NO_MEMORY;
}
@@ -1659,31 +1760,40 @@ int handle_smcp10_cmd_take_step(mcu_event_queue target, mcu_event *pEvent)
// Resolve params
mcu_error retval = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSbHdr, &pSbData);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
- if (pSbHdr->size >= 7) {
+ if (pSbHdr->size >= 7)
+ {
systemInfo *info = mcu_commonGetSystemConf(false);
// Distribute event to all relevant actuators
- if (target == MCU_EVENT_QUEUE_COMM) {
+ if (target == MCU_EVENT_QUEUE_COMM)
+ {
- for (int act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++) {
+ for (int act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++)
+ {
// Append the event in target executor queue if data differs from zero
- if (mcu_commonIsActuatorEnabled(act, info)) {
- int32_t step_nm = *(int32_t*)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)act);
- int32_t speed_ums = *(int32_t*)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)act+4);
- if (step_nm != 0 && speed_ums > 0) {
+ if (mcu_commonIsActuatorEnabled(act, info))
+ {
+ int32_t step_nm = *(int32_t *)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)act);
+ int32_t speed_ums = *(int32_t *)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)act + 4);
+ if (step_nm != 0 && speed_ums > 0)
+ {
retval = mcu_executorInsertEvent(act, pEvent);
- if (retval != MCU_ERROR_NONE) {
+ if (retval != MCU_ERROR_NONE)
+ {
break;
}
}
}
}
- } else {
+ }
+ else
+ {
// Get step len and speed
- int32_t step_nm = *(int32_t*)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)target);
- int32_t speed_ums = *(int32_t*)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)target+4);
+ int32_t step_nm = *(int32_t *)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)target);
+ int32_t speed_ums = *(int32_t *)smcpv10_get_data(pSbHdr, pSbData, (uint32_t)target + 4);
/*
* // TODO limit checks
@@ -1699,20 +1809,23 @@ int handle_smcp10_cmd_take_step(mcu_event_queue target, mcu_event *pEvent)
}
}
*/
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
retval = integrationTakeStep((mcu_actuator)target, speed_ums, step_nm);
}
}
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_UNDERFLOW;
}
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_PARAMETER;
}
return retval;
-
-
}
int handle_smcp10_cmd_take_legacy_step(mcu_event_queue target, mcu_event *pEvent)
@@ -1723,37 +1836,50 @@ int handle_smcp10_cmd_take_legacy_step(mcu_event_queue target, mcu_event *pEvent
// Resolve params
mcu_error retVal = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSbHdr, &pSbData);
- if (retVal == MCU_ERROR_NONE) {
- if (pSbHdr->size >= 2) {
- int32_t *pSpeed = (int32_t*)smcpv10_get_data(pSbHdr, pSbData, 1);
+ if (retVal == MCU_ERROR_NONE)
+ {
+ if (pSbHdr->size >= 2)
+ {
+ int32_t *pSpeed = (int32_t *)smcpv10_get_data(pSbHdr, pSbData, 1);
- if (target == MCU_EVENT_QUEUE_COMM) {
- systemInfo * pSysInfo = mcu_commonGetSystemConf(false);
+ if (target == MCU_EVENT_QUEUE_COMM)
+ {
+ systemInfo *pSysInfo = mcu_commonGetSystemConf(false);
- int lastAct = pSbHdr->size-1;
- if (lastAct > ACTUATOR_LAST) {
+ int lastAct = pSbHdr->size - 1;
+ if (lastAct > ACTUATOR_LAST)
+ {
lastAct = ACTUATOR_LAST;
}
- for (int act = ACTUATOR_FIRST; act <= lastAct; act++) {
- if (*pSpeed) {
- if (mcu_commonIsActuatorEnabled(act, pSysInfo)) {
- mcu_error status = mcu_executorInsertEvent((mcu_event_queue) act, pEvent);
- if (status != MCU_ERROR_NONE) {
+ for (int act = ACTUATOR_FIRST; act <= lastAct; act++)
+ {
+ if (*pSpeed)
+ {
+ if (mcu_commonIsActuatorEnabled(act, pSysInfo))
+ {
+ mcu_error status = mcu_executorInsertEvent((mcu_event_queue)act, pEvent);
+ if (status != MCU_ERROR_NONE)
+ {
retVal = status;
}
}
}
pSpeed++;
}
- } else {
+ }
+ else
+ {
pSpeed += target;
- if (*pSpeed) {
- int32_t gear = *(int32_t*)smcpv10_get_data(pSbHdr, pSbData, 0);
+ if (*pSpeed)
+ {
+ int32_t gear = *(int32_t *)smcpv10_get_data(pSbHdr, pSbData, 0);
retVal = integrationTakeLegacyStep((mcu_actuator)target, gear, *pSpeed);
- }
+ }
}
- } else {
+ }
+ else
+ {
retVal = MCU_ERROR_PARAMETER;
}
}
@@ -1770,60 +1896,75 @@ int handle_smcp10_cmd_store_mem(const mcu_event_queue target, mcu_event *pEvent)
// Resolve params
retval = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSb, &pSbData);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
- if (pSb->size >= 1) {
+ if (pSb->size >= 1)
+ {
int act;
int32_t position;
- uint32_t index = *(uint32_t*)smcpv10_get_data(pSb, pSbData, 0);
+ uint32_t index = *(uint32_t *)smcpv10_get_data(pSb, pSbData, 0);
systemInfo *info = mcu_commonGetSystemConf(false);
- for (act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++) {
+ for (act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++)
+ {
- if (mcu_commonIsActuatorEnabled(act, info)) {
+ if (mcu_commonIsActuatorEnabled(act, info))
+ {
// Read the current position of an actuator ...
retval = mcu_actuatorGetPosition(act, (int *)&position, true);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
// ... and store it.
- retval = integrationSetMemDriveTarget((mcu_actuator) act, index, position);
+ retval = integrationSetMemDriveTarget((mcu_actuator)act, index, position);
}
- if (retval != MCU_ERROR_NONE) {
+ if (retval != MCU_ERROR_NONE)
+ {
break;
}
}
}
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_PARAMETER;
}
}
MCUTRACE(("%s (%d)\n", __func__, retval));
- return retval;
+ return retval;
}
-int handle_smcp10_cmd_goto_mem(const mcu_event_queue target, mcu_event *pEvent) {
+int handle_smcp10_cmd_goto_mem(const mcu_event_queue target, mcu_event *pEvent)
+{
UNUSED(target);
bool reverseOrder = false;
int retval = MCU_ERROR_NONE;
// Resolve drive order
- if (target == MCU_EVENT_QUEUE_COMM) {
+ if (target == MCU_EVENT_QUEUE_COMM)
+ {
char *pSbData;
smcp1_sb_header *pSb;
retval = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSb, &pSbData);
- if (retval == MCU_ERROR_NONE) {
- if (pSb->size >= 1) {
- uint32_t mem_index = *(uint32_t*)smcpv10_get_data(pSb, pSbData, 0);
+ if (retval == MCU_ERROR_NONE)
+ {
+ if (pSb->size >= 1)
+ {
+ uint32_t mem_index = *(uint32_t *)smcpv10_get_data(pSb, pSbData, 0);
retval = smcp10_resolve_mem_drive_order(mem_index, &reverseOrder);
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_PARAMETER;
}
}
}
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
retval = do_handle_smcp10_cmd_goto_mem(target, pEvent, reverseOrder);
}
@@ -1842,39 +1983,50 @@ static mcu_error smcp10_forward_next_target(const mcu_event_queue current,
mcu_error retval = mcu_smcp10_read_param(SMCP1_PARAM_AXIS_DRIVE_ORDER,
(int *)&driveOrder, false);
- if (reverseOrder) {
+ if (reverseOrder)
+ {
uint32_t reverse = mcu_swap32(driveOrder);
driveOrder = reverse;
}
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
uint32_t i;
systemInfo *info = mcu_commonGetSystemConf(false);
uint8_t *pAct = (uint8_t *)&driveOrder;
retval = MCU_ERROR_NOT_FOUND;
- if (current == MCU_EVENT_QUEUE_COMM) {
- for (i = 0; i < sizeof driveOrder; i++) {
- if (mcu_commonIsActuatorEnabled(*pAct, info) && (mask & (1 << *pAct))) {
+ if (current == MCU_EVENT_QUEUE_COMM)
+ {
+ for (i = 0; i < sizeof driveOrder; i++)
+ {
+ if (mcu_commonIsActuatorEnabled(*pAct, info) && (mask & (1 << *pAct)))
+ {
retval = mcu_executorInsertEvent((mcu_event_queue)*pAct, pEvent);
break;
}
pAct++;
}
- } else {
+ }
+ else
+ {
// Forward the event to the next actuator
bool foundmyself = false;
- for (i = 0; i < sizeof driveOrder; i++) {
- if (!foundmyself && (mcu_event_queue)*pAct == current) {
- foundmyself = true;// Next is the one we need
+ for (i = 0; i < sizeof driveOrder; i++)
+ {
+ if (!foundmyself && (mcu_event_queue)*pAct == current)
+ {
+ foundmyself = true; // Next is the one we need
pAct++;
continue;
}
// Search the next target actuator
- if (foundmyself) {
- if (mcu_commonIsActuatorEnabled(*pAct, info) && (mask & (1 << *pAct))) {
+ if (foundmyself)
+ {
+ if (mcu_commonIsActuatorEnabled(*pAct, info) && (mask & (1 << *pAct)))
+ {
retval = mcu_executorInsertEvent((mcu_event_queue)*pAct, pEvent);
break;
}
@@ -1891,7 +2043,7 @@ int handle_smcp10_cmd_goto_mem_reverse(const mcu_event_queue target, mcu_event *
return do_handle_smcp10_cmd_goto_mem(target, pEvent, true);
}
-#define SMCP10_MASK_ALL_ACTUATORS 0b1111
+#define SMCP10_MASK_ALL_ACTUATORS 0b1111
static int do_handle_smcp10_cmd_goto_mem(const mcu_event_queue target, mcu_event *pEvent, bool reverseOrder)
{
char *pSbData;
@@ -1900,74 +2052,97 @@ static int do_handle_smcp10_cmd_goto_mem(const mcu_event_queue target, mcu_event
// Resolve params
mcu_error retval = mcu_smcpv1_get_sub_block_data(pEvent, 0, &pSb, &pSbData);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
bool simultant_drive = false;
- if (pSb->size >= 3) {
- uint32_t axis_drive_mode = *(uint32_t*)smcpv10_get_data(pSb, pSbData, 2);
- if (axis_drive_mode == 1) {
+ if (pSb->size >= 3)
+ {
+ uint32_t axis_drive_mode = *(uint32_t *)smcpv10_get_data(pSb, pSbData, 2);
+ if (axis_drive_mode == 1)
+ {
simultant_drive = true;
}
}
uint32_t targetMask = SMCP10_MASK_ALL_ACTUATORS;
- if (pSb->size >= 5) {
- targetMask = *(uint32_t*)smcpv10_get_data(pSb, pSbData, 4);
+ if (pSb->size >= 5)
+ {
+ targetMask = *(uint32_t *)smcpv10_get_data(pSb, pSbData, 4);
}
- if (target == MCU_EVENT_QUEUE_COMM) {
+ if (target == MCU_EVENT_QUEUE_COMM)
+ {
// Set the command type based on the drive order
- if (reverseOrder) {
+ if (reverseOrder)
+ {
pEvent->frame.type = SMCP1_CMD_GOTO_MEM_REVERSE;
- } else {
+ }
+ else
+ {
pEvent->frame.type = SMCP1_CMD_GOTO_MEM;
}
- if (!simultant_drive) {
+ if (!simultant_drive)
+ {
// ... forward the modified message to the first actuator.
- retval = smcp10_forward_next_target(target, pEvent, reverseOrder,targetMask);
- } else {
+ retval = smcp10_forward_next_target(target, pEvent, reverseOrder, targetMask);
+ }
+ else
+ {
// ... forward the modified message to all actuators simultaneously
mcu_actuator act;
systemInfo *info = mcu_commonGetSystemConf(false);
// Forward the message to all executors at the same time.
- for (act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++) {
- if (mcu_commonIsActuatorEnabled(act, info)) {
+ for (act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++)
+ {
+ if (mcu_commonIsActuatorEnabled(act, info))
+ {
// ... append
retval = mcu_executorInsertEvent(act, pEvent);
- if (retval != MCU_ERROR_NONE) {
+ if (retval != MCU_ERROR_NONE)
+ {
break;
}
}
}
}
- } else {
+ }
+ else
+ {
- if (pSb->size >= 2) {
+ if (pSb->size >= 2)
+ {
int targetPos;
- uint32_t memIndex = *(uint32_t*)smcpv10_get_data(pSb, pSbData, 0);
- int speed = *(int*)smcpv10_get_data(pSb, pSbData, 1);
+ uint32_t memIndex = *(uint32_t *)smcpv10_get_data(pSb, pSbData, 0);
+ int speed = *(int *)smcpv10_get_data(pSb, pSbData, 1);
- retval = mcu_smcp10_read_mem_drive_target((mcu_actuator)target, memIndex, (int32_t*)&targetPos);
+ retval = mcu_smcp10_read_mem_drive_target((mcu_actuator)target, memIndex, (int32_t *)&targetPos);
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
// Finally. Process the operation
- retval = integrationGotoPosition((mcu_actuator) target, targetPos, speed, simultant_drive);
+ retval = integrationGotoPosition((mcu_actuator)target, targetPos, speed, simultant_drive);
- if (!simultant_drive) {
+ if (!simultant_drive)
+ {
// Forward the event to next target
- if (retval == MCU_ERROR_NONE) {
+ if (retval == MCU_ERROR_NONE)
+ {
mcu_error status = smcp10_forward_next_target(target, pEvent, reverseOrder, targetMask);
- if (status != MCU_ERROR_NOT_FOUND) {
+ if (status != MCU_ERROR_NOT_FOUND)
+ {
retval = status;
}
}
}
}
- } else {
+ }
+ else
+ {
retval = MCU_ERROR_PARAMETER;
}
}
@@ -1975,21 +2150,26 @@ static int do_handle_smcp10_cmd_goto_mem(const mcu_event_queue target, mcu_event
return retval;
}
-int handle_smcp10_cmd_stop(const mcu_event_queue target, mcu_event *pEvent) {
+int handle_smcp10_cmd_stop(const mcu_event_queue target, mcu_event *pEvent)
+{
UNUSED(target);
UNUSED(pEvent);
mcu_error retVal = MCU_ERROR_NONE;
systemInfo *info = mcu_commonGetSystemConf(false);
- for (mcu_actuator act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++) {
- if (mcu_commonIsActuatorEnabled(act, info)) {
+ for (mcu_actuator act = ACTUATOR_FIRST; act <= ACTUATOR_LAST; act++)
+ {
+ if (mcu_commonIsActuatorEnabled(act, info))
+ {
// Flush actuator event queues
mcu_error status = mcu_executorRemoveAll(act);
- if (status == MCU_ERROR_NONE) {
+ if (status == MCU_ERROR_NONE)
+ {
// Stop the move
- status = integrationStop(act);
- if (status != MCU_ERROR_NONE) {
+ status = integrationStop(act);
+ if (status != MCU_ERROR_NONE)
+ {
retVal = status;
}
}