Merge pull request #51 from hefloryd/feature/bugfix

Minor bugfixes

CMakeLists.txt

@@ -7,7 +7,7 @@ project (SOES)
 
 set (SOES_VERSION_MAJOR 2)
 set (SOES_VERSION_MINOR 1)
-set (SOES_VERSION_PATCH 5)
+set (SOES_VERSION_PATCH 6)
 
 # Generate version numbers
 configure_file (

cmake/Platform/rt-kernel.cmake

@@ -41,7 +41,7 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${MACHINE_FLAGS}" CACHE STRING "" FORCE)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${MACHINE_FLAGS}" CACHE STRING "" FORCE)
 
 # Default libs
-set(RTK_LIBS "-l${BSP} -l${ARCH} -lkern -ldev -lsio -lblock -lfs -lusb -llwip -lptpd -leth -li2c -lrtc -lcan -lnand -lspi -lnor -lpwm -ladc -ltrace -lcounter -lc -lm")
+set(RTK_LIBS "-l${BSP} -l${ARCH} -lkern -ldev -lsio -lblock -lfs -lusb -llwip -lptpd -leth -li2c -lrtc -lcan -lnand -lspi -lnor -lpwm -ladc -ltrace -lcounter -lc -lm -lshell")
 
 set(CMAKE_CXX_LINK_EXECUTABLE "<CMAKE_CXX_COMPILER> <FLAGS> <CMAKE_CXX_LINK_FLAGS> <LINK_FLAGS> <OBJECTS> -o <TARGET> -nostartfiles -T${RTK}/bsp/${BSP}/${BSP}.ld -Wl,--start-group <LINK_LIBRARIES> ${RTK_LIBS} -Wl,--end-group")
 set(CMAKE_C_LINK_EXECUTABLE   "<CMAKE_C_COMPILER>   <FLAGS> <CMAKE_C_LINK_FLAGS>   <LINK_FLAGS> <OBJECTS> -o <TARGET> -nostartfiles -T${RTK}/bsp/${BSP}/${BSP}.ld -Wl,--start-group <LINK_LIBRARIES> ${RTK_LIBS} -Wl,--end-group")

soes/esc.c

@@ -685,6 +685,12 @@ uint8_t ESC_checkSM23 (uint8_t state)
       /* fail state change */
       return (ESCpreop | ESCerror);
    }
+   if ((ESC_SM2_sma + (etohs (SM->Length) * 3)) > ESC_SM3_sma)
+   {
+      ESCvar.SMtestresult = SMRESULT_ERRSM2;
+      /* SM2 overlaps SM3, fail state change */
+      return (ESCpreop | ESCerror);
+   }
    ESC_read (ESCREG_SM3, (void *) &ESCvar.SM[3], sizeof (ESCvar.SM[3]));
    SM = (_ESCsm2 *) & ESCvar.SM[3];
    if ((etohs (SM->PSA) != ESC_SM3_sma) || (etohs (SM->Length) != ESCvar.ESC_SM3_sml)

soes/esc.h

@@ -340,8 +340,7 @@ typedef struct CC_PACKED
 } _ESCsmCompact;
 CC_PACKED_END
 
-CC_PACKED_BEGIN
-typedef struct CC_PACKED
+typedef struct
 {
    /* Configuration input is saved so the user variable may go out-of-scope */
    int use_interrupt;
@@ -390,20 +389,10 @@ typedef struct CC_PACKED
    uint16_t frags;
    uint16_t fragsleft;
 
-#if defined(EC_LITTLE_ENDIAN)
-   uint8_t r1:1;
-   uint8_t toggle:1;
-   uint8_t r2:6;
-#endif
+   uint8_t toggle;
 
-#if defined(EC_BIG_ENDIAN)
-   uint8_t r2:6;
-   uint8_t toggle:1;
-   uint8_t r1:1;
-#endif
 
    uint8_t SMtestresult;
-   int16_t temp;
    uint32_t PrevTime;
    _ESCsm SM[4];
    /* Volatile since it may be read from ISR */
@@ -413,7 +402,6 @@ typedef struct CC_PACKED
    volatile int8_t synccounter;
    volatile _App App;
 } _ESCvar;
-CC_PACKED_END
 
 CC_PACKED_BEGIN
 typedef struct CC_PACKED

soes/esc_coe.c

@@ -466,9 +466,10 @@ void SDO_download (void)
       if (nsub >= 0)
       {
          objd = SDOobjects[nidx].objdesc;
-         if (((objd + nsub)->access == ATYPE_RW) ||
-             (((objd + nsub)->access == ATYPE_RWpre)
-              && ((ESCvar.ALstatus & 0x0f) == ESCpreop)))
+         uint8_t access = (objd + nsub)->access & 0x3f;
+         uint8_t state = ESCvar.ALstatus & 0x0f;
+         if (access == ATYPE_RW ||
+             (access == ATYPE_RWpre && state == ESCpreop))
          {
             /* expedited? */
             if (coesdo->command & COE_EXPEDITED_INDICATOR)
@@ -513,7 +514,7 @@ void SDO_download (void)
          }
          else
          {
-            if ((objd + nsub)->access == ATYPE_RWpre)
+            if (access == ATYPE_RWpre)
             {
                SDO_abort (index, subindex, ABORT_NOTINTHISSTATE);
             }
@@ -557,7 +558,8 @@ void SDO_infoerror (uint32_t abortcode)
       coeres->infoheader.incomplete = 0;
       coeres->infoheader.reserved = 0x00;
       coeres->infoheader.fragmentsleft = 0;
-      coeres->index = htoel (abortcode);
+      coeres->index = (uint16_t)htoel (abortcode);
+      coeres->datatype = (uint16_t)(htoel (abortcode) >> 16);
       MBXcontrol[MBXout].state = MBXstate_outreq;
       MBXcontrol[0].state = MBXstate_idle;
       ESCvar.xoe = 0;
@@ -741,12 +743,20 @@ void SDO_getod (void)
             int32_t nsub = SDO_findsubindex (nidx, 0);
             const _objd *objd = SDOobjects[nidx].objdesc;
             coel->datatype = htoes ((objd + nsub)->datatype);
+            coel->maxsub = SDOobjects[nidx].maxsub;
+         }
+         else if (SDOobjects[nidx].objtype == OTYPE_ARRAY)
+         {
+            int32_t nsub = SDO_findsubindex (nidx, 0);
+            const _objd *objd = SDOobjects[nidx].objdesc;
+            coel->datatype = htoes ((objd + nsub)->datatype);
+            coel->maxsub = SDOobjects[nidx].objdesc->value;
          }
          else
          {
             coel->datatype = htoes (0);
+            coel->maxsub = SDOobjects[nidx].objdesc->value;
          }
-         coel->maxsub = SDOobjects[nidx].maxsub;
          coel->objectcode = SDOobjects[nidx].objtype;
          s = (uint8_t *) SDOobjects[nidx].name;
          d = (uint8_t *) &(coel->name);

soes/hal/rt-kernel-twrk60/esc_hw.c

@@ -16,6 +16,8 @@
 #include <string.h>
 #include <gpio.h>
 
+#define MAX_READ_SIZE   128
+
 #define ESC_CMD_READ    0x02
 #define ESC_CMD_READWS  0x03
 #define ESC_CMD_WRITE   0x04
@@ -24,7 +26,7 @@
 #define ESC_NEXT        0x00
 
 static int et1100 = -1;
-static uint8_t read_termination[MAX(sizeof(Wb), 128)] = { 0 };
+static uint8_t read_termination[MAX_READ_SIZE] = { 0 };
 
 #define GPIO_ECAT_RESET    1 /* specific function to hold ESC reset on startup
                               * when emulating EEPROM
@@ -56,7 +58,7 @@ static void esc_address (uint16_t address, uint8_t command)
  */
 void ESC_read (uint16_t address, void *buf, uint16_t len)
 {
-   ASSERT(len <= sizeof(read_termination));
+   ASSERT(len <= MAX_READ_SIZE);
 
    /* Select device. */
    spi_select (et1100);
@@ -69,7 +71,7 @@ void ESC_read (uint16_t address, void *buf, uint16_t len)
     * Read (and write termination bytes).
     */
    spi_bidirectionally_transfer (et1100, buf, read_termination +
-                                 (sizeof(read_termination) - len), len);
+                                 (MAX_READ_SIZE - len), len);
 
    /* Un-select device. */
    spi_unselect (et1100);
@@ -122,5 +124,5 @@ void ESC_init (const esc_cfg_t * config)
 {
    const char * spi_name = (char *)config->user_arg;
    et1100 = open (spi_name, O_RDWR, 0);
-   read_termination[sizeof(read_termination) - 1] = 0xFF;
+   read_termination[MAX_READ_SIZE - 1] = 0xFF;
 }

soes/include/sys/gcc/cc.h

@@ -33,7 +33,11 @@ extern "C"
 #define CC_PACKED_END
 #define CC_PACKED       __attribute__((packed))
 
+#ifdef __rtk__
+#define CC_ASSERT(exp) ASSERT (exp)
+#else
 #define CC_ASSERT(exp) assert (exp)
+#endif
 #define CC_STATIC_ASSERT(exp) _Static_assert (exp, "")
 
 #define CC_DEPRECATED   __attribute__((deprecated))
@@ -66,7 +70,8 @@ extern "C"
 #endif
 
 #ifdef ESC_DEBUG
-#define DPRINT(...) rprintp ("esc_foe: "__VA_ARGS__) /* TODO */
+#include <rprint.h>
+#define DPRINT(...) rprintp ("soes: "__VA_ARGS__) /* TODO */
 #else
 #define DPRINT(...)
 #endif  /* DEBUG */