diff --git a/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml b/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml index 407a034..6e61434 100755 --- a/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml +++ b/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml @@ -389,7 +389,7 @@ DT6005 - 48 + 80 0 Max SubIndex @@ -403,8 +403,8 @@ 1 ActualPosition - DINT - 32 + LREAL + 64 16 ro @@ -414,7 +414,7 @@ DT7002 - 48 + 80 0 Max SubIndex @@ -428,8 +428,8 @@ 1 CommandedPosition - DINT - 32 + LREAL + 64 16 ro @@ -458,8 +458,8 @@ 16 - REAL - 32 + LREAL + 64 UDINT @@ -623,7 +623,7 @@ CommandedPosition - #x70020120 + #x70020140 @@ -646,7 +646,7 @@ EncPos - #x60000020 + #x60000040 @@ -669,7 +669,7 @@ EncFrequency - #x60010020 + #x60010040 @@ -761,7 +761,7 @@ ActualPosition - #x60050120 + #x60050140 @@ -901,8 +901,8 @@ #x6000 EncPos - REAL - 32 + LREAL + 64 0 @@ -914,8 +914,8 @@ #x6001 EncFrequency - REAL - 32 + LREAL + 64 0 @@ -967,7 +967,7 @@ #x6005 StepGenOut1 DT6005 - 48 + 80 Max SubIndex @@ -1016,7 +1016,7 @@ #x7002 StepGenIn1 DT7002 - 48 + 80 Max SubIndex @@ -1073,9 +1073,9 @@ #x7002 #x1 - 32 + 64 CommandedPosition - DINT + LREAL @@ -1084,9 +1084,9 @@ #x6000 #x0 - 32 + 64 EncPos - REAL + LREAL @@ -1095,9 +1095,9 @@ #x6001 #x0 - 32 + 64 EncFrequency - REAL + LREAL @@ -1139,9 +1139,9 @@ #x6005 #x1 - 32 + 64 ActualPosition - DINT + LREAL diff --git a/Firmware/lib/soes/esi.json b/Firmware/lib/soes/esi.json index 7e6c7f7..d84c06b 100755 --- a/Firmware/lib/soes/esi.json +++ b/Firmware/lib/soes/esi.json @@ -57,7 +57,7 @@ "pdo_mappings": [ "txpdo" ], - "dtype": "REAL32", + "dtype": "REAL64", "value": "0", "data": "&Obj.EncPos" }, @@ -68,7 +68,7 @@ "pdo_mappings": [ "txpdo" ], - "dtype": "REAL32", + "dtype": "REAL64", "value": "0", "data": "&Obj.EncFrequency" }, @@ -115,7 +115,7 @@ }, { "name": "ActualPosition", - "dtype": "INTEGER32", + "dtype": "REAL64", "data": "&Obj.StepGenOut1.ActualPosition", "value": "0", "access": "RO" @@ -159,7 +159,7 @@ }, { "name": "CommandedPosition", - "dtype": "INTEGER32", + "dtype": "REAL64", "data": "&Obj.StepGenIn1.CommandedPosition", "value": "0", "access": "RO" diff --git a/Firmware/lib/soes/objectlist.c b/Firmware/lib/soes/objectlist.c index e4db87c..88b4215 100755 --- a/Firmware/lib/soes/objectlist.c +++ b/Firmware/lib/soes/objectlist.c @@ -110,17 +110,17 @@ const _objd SDO1601[] = const _objd SDO1602[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 1, NULL}, - {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_01, 0x70020120, NULL}, + {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_01, 0x70020140, NULL}, }; const _objd SDO1A00[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A00_00, 1, NULL}, - {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A00_01, 0x60000020, NULL}, + {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A00_01, 0x60000040, NULL}, }; const _objd SDO1A01[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A01_00, 1, NULL}, - {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A01_01, 0x60010020, NULL}, + {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A01_01, 0x60010040, NULL}, }; const _objd SDO1A02[] = { @@ -140,7 +140,7 @@ const _objd SDO1A04[] = const _objd SDO1A05[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A05_00, 1, NULL}, - {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A05_01, 0x60050120, NULL}, + {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A05_01, 0x60050140, NULL}, }; const _objd SDO1C00[] = { @@ -169,11 +169,11 @@ const _objd SDO1C13[] = }; const _objd SDO6000[] = { - {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6000, 0x00000000, &Obj.EncPos}, + {0x0, DTYPE_REAL64, 64, ATYPE_RO | ATYPE_TXPDO, acName6000, 0, &Obj.EncPos}, }; const _objd SDO6001[] = { - {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6001, 0x00000000, &Obj.EncFrequency}, + {0x0, DTYPE_REAL64, 64, ATYPE_RO | ATYPE_TXPDO, acName6001, 0, &Obj.EncFrequency}, }; const _objd SDO6002[] = { @@ -190,7 +190,7 @@ const _objd SDO6004[] = const _objd SDO6005[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName6005_00, 1, NULL}, - {0x01, DTYPE_INTEGER32, 32, ATYPE_RO, acName6005_01, 0, &Obj.StepGenOut1.ActualPosition}, + {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName6005_01, 0, &Obj.StepGenOut1.ActualPosition}, }; const _objd SDO7000[] = { @@ -203,7 +203,7 @@ const _objd SDO7001[] = const _objd SDO7002[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 1, NULL}, - {0x01, DTYPE_INTEGER32, 32, ATYPE_RO, acName7002_01, 0, &Obj.StepGenIn1.CommandedPosition}, + {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7002_01, 0, &Obj.StepGenIn1.CommandedPosition}, }; const _objectlist SDOobjects[] = diff --git a/Firmware/lib/soes/utypes.h b/Firmware/lib/soes/utypes.h index 9f77c40..971c1e8 100755 --- a/Firmware/lib/soes/utypes.h +++ b/Firmware/lib/soes/utypes.h @@ -13,14 +13,14 @@ typedef struct /* Inputs */ - float EncPos; - float EncFrequency; + double EncPos; + double EncFrequency; uint32_t DiffT; uint32_t IndexByte; uint32_t IndexStatus; struct { - int32_t ActualPosition; + double ActualPosition; } StepGenOut1; /* Outputs */ @@ -29,7 +29,7 @@ typedef struct uint32_t IndexLatchEnable; struct { - int32_t CommandedPosition; + double CommandedPosition; } StepGenIn1; } _Objects; diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 1d87fce..5673cae 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -106,8 +106,8 @@ static esc_cfg_t config = #define STEPPER_STEP_PIN PA11 HardwareTimer *MyTim; volatile uint32_t stepCount = 0, stepPulses = 0; -volatile int32_t actualPosition = 0; -volatile int32_t requestedPosition; +volatile double_t actualPosition = 0; +volatile double_t requestedPosition; volatile uint32_t pulsesToGo = 0; volatile byte forwardDirection = 0; // 1 if going forward @@ -119,7 +119,7 @@ void TimerStep_CB(void) stepCount++; else stepCount--; - //digitalWrite(STEPPER_STEP_PIN, !digitalRead(STEPPER_STEP_PIN)); + // digitalWrite(STEPPER_STEP_PIN, !digitalRead(STEPPER_STEP_PIN)); if (stepCount == stepPulses) { MyTim->pause(); @@ -146,7 +146,7 @@ void setup(void) MyTim->attachInterrupt(TimerStep_CB); stepCount = 0; pinMode(STEPPER_DIR_PIN, OUTPUT); -#if 1 +#if 0 while (1) { @@ -223,21 +223,19 @@ void indexPulse(void) void sync0Handler(void) { + const int32_t stepsPerMM = 100; // Update the actual position - actualPosition += pulsesToGo; - Obj.StepGenOut1.ActualPosition = actualPosition; + pulsesToGo = stepsPerMM * (Obj.StepGenIn1.CommandedPosition - Obj.StepGenOut1.ActualPosition); + Obj.StepGenOut1.ActualPosition = Obj.StepGenIn1.CommandedPosition; // Cheat // Get new end position - requestedPosition = Obj.StepGenIn1.CommandedPosition; - // Get the diff and the direction - pulsesToGo = requestedPosition - actualPosition; syncCounts++; - forwardDirection = syncCounts % 2 ? 1 : 0; + forwardDirection = pulsesToGo > 1 ? 1 : 0; // Set direction pin Obj.DiffT = forwardDirection; digitalWrite(STEPPER_DIR_PIN, forwardDirection); // I think one should really wait a bit when changed // Make the pulses using hardware timer - makePulses(sync0CycleTime / 1000, pulsesToGo); + makePulses(sync0CycleTime / 1000, abs((int)pulsesToGo)); } void ESC_interrupt_enable(uint32_t mask) diff --git a/linuxcnc/ethercat-conf.xml b/linuxcnc/ethercat-conf.xml index 674957a..618f5c5 100644 --- a/linuxcnc/ethercat-conf.xml +++ b/linuxcnc/ethercat-conf.xml @@ -1,56 +1,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -59,14 +9,17 @@ + + + - + - - + + @@ -76,7 +29,10 @@ - + + + + diff --git a/linuxcnc/lcec-64bitfloatadded.patch b/linuxcnc/lcec-64bitfloatadded.patch index e34256f..a5ce83a 100644 --- a/linuxcnc/lcec-64bitfloatadded.patch +++ b/linuxcnc/lcec-64bitfloatadded.patch @@ -1,52 +1,102 @@ diff --git a/src/lcec_conf.c b/src/lcec_conf.c -index b0e9ad1..607c6d7 100644 +index b0e9ad1..6cc7f8a 100644 --- a/src/lcec_conf.c +++ b/src/lcec_conf.c @@ -1267,6 +1267,11 @@ static void parsePdoEntryAttrs(LCEC_CONF_XML_INST_T *inst, int next, const char p->halType = HAL_FLOAT; continue; } -+ if (strcasecmp(val, "float-lreal") == 0) { -+ p->subType = lcecPdoEntTypeFloatLREAL; ++ if (strcasecmp(val, "float-double") == 0) { ++ p->subType = lcecPdoEntTypeFloatDouble; + p->halType = HAL_FLOAT; + continue; + } fprintf(stderr, "%s: ERROR: Invalid pdoEntry halType %s\n", modname, val); XML_StopParser(inst->parser, 0); return; +@@ -1410,6 +1415,11 @@ static void parseComplexEntryAttrs(LCEC_CONF_XML_INST_T *inst, int next, const c + p->halType = HAL_FLOAT; + continue; + } ++ if (strcasecmp(val, "float-double") == 0) { ++ p->subType = lcecPdoEntTypeFloatDouble; ++ p->halType = HAL_FLOAT; ++ continue; ++ } + fprintf(stderr, "%s: ERROR: Invalid complexEntry halType %s\n", modname, val); + XML_StopParser(inst->parser, 0); + return; diff --git a/src/lcec_conf.h b/src/lcec_conf.h -index 5d9943c..2620938 100644 +index 5d9943c..261c965 100644 --- a/src/lcec_conf.h +++ b/src/lcec_conf.h -@@ -56,7 +56,8 @@ typedef enum { +@@ -55,6 +55,7 @@ typedef enum { + lcecPdoEntTypeSimple, lcecPdoEntTypeFloatSigned, lcecPdoEntTypeFloatUnsigned, ++ lcecPdoEntTypeFloatDouble, lcecPdoEntTypeComplex, -- lcecPdoEntTypeFloatIeee -+ lcecPdoEntTypeFloatIeee, -+ lcecPdoEntTypeFloatLREAL + lcecPdoEntTypeFloatIeee } LCEC_PDOENT_TYPE_T; - - typedef enum { diff --git a/src/lcec_generic.c b/src/lcec_generic.c -index dfddf73..dde7164 100644 +index dfddf73..2f58fde 100644 --- a/src/lcec_generic.c +++ b/src/lcec_generic.c -@@ -78,7 +78,7 @@ int lcec_generic_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t - +@@ -26,6 +26,7 @@ hal_s32_t lcec_generic_read_s32(uint8_t *pd, lcec_generic_pin_t *hal_data); + hal_u32_t lcec_generic_read_u32(uint8_t *pd, lcec_generic_pin_t *hal_data); + void lcec_generic_write_s32(uint8_t *pd, lcec_generic_pin_t *hal_data, hal_s32_t sval); + void lcec_generic_write_u32(uint8_t *pd, lcec_generic_pin_t *hal_data, hal_u32_t uval); ++void lcec_generic_write_f64(uint8_t *pd, lcec_generic_pin_t *hal_data, hal_float_t fval); + + int lcec_generic_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) { + lcec_master_t *master = slave->master; +@@ -78,7 +79,7 @@ int lcec_generic_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t + case HAL_FLOAT: // check data size - if (hal_data->bitLength > 32) { -+ if (!(hal_data->bitLength == 32 || hal_data->bitLength == 64)) { ++ if (hal_data->bitLength > 64) { rtapi_print_msg(RTAPI_MSG_WARN, LCEC_MSG_PFX "unable to export pin %s.%s.%s.%s: invalid process data bitlen!\n", LCEC_MODULE_NAME, master->name, slave->name, hal_data->name); continue; } -@@ -133,6 +133,8 @@ void lcec_generic_read(struct lcec_slave *slave, long period) { +@@ -133,6 +134,8 @@ void lcec_generic_read(struct lcec_slave *slave, long period) { fval = lcec_generic_read_u32(pd, hal_data); } else if(hal_data->subType == lcecPdoEntTypeFloatIeee){ fval = EC_READ_REAL(&pd[hal_data->pdo_os]); -+ } else if(hal_data->subType == lcecPdoEntTypeFloatLREAL){ ++ } else if(hal_data->subType == lcecPdoEntTypeFloatDouble){ + fval = EC_READ_LREAL(&pd[hal_data->pdo_os]); } else { fval = lcec_generic_read_s32(pd, hal_data); } +@@ -185,6 +188,8 @@ void lcec_generic_write(struct lcec_slave *slave, long period) { + + if (hal_data->subType == lcecPdoEntTypeFloatUnsigned) { + lcec_generic_write_u32(pd, hal_data, (hal_u32_t) fval); ++ } else if (hal_data->subType == lcecPdoEntTypeFloatDouble) { ++ lcec_generic_write_f64(pd, hal_data, fval); + } else { + lcec_generic_write_s32(pd, hal_data, (hal_s32_t) fval); + } +@@ -300,3 +305,22 @@ void lcec_generic_write_u32(uint8_t *pd, lcec_generic_pin_t *hal_data, hal_u32_t + } + } + ++void lcec_generic_write_f64(uint8_t *pd, lcec_generic_pin_t *hal_data, hal_float_t fval) { ++ int i, offset; ++ ++ if (hal_data->pdo_bp == 0 && hal_data->bitOffset == 0) { ++ EC_WRITE_LREAL(&pd[hal_data->pdo_os], fval); ++ } ++ union { ++ double d; ++ uin64_t u; ++ } v; ++ v.d = fval; ++ offset = ((hal_data->pdo_os << 3) | (hal_data->pdo_bp & 0x07)) + hal_data->bitOffset; ++ for (i=0; i < hal_data->bitLength; i++, offset++) { ++ EC_WRITE_BIT(&pd[offset >> 3], offset & 0x07,v.u & 1); ++ v.u >>= 1; ++ } ++#endif ++} ++