103 lines
4.1 KiB
Diff
103 lines
4.1 KiB
Diff
diff --git a/src/lcec_conf.c b/src/lcec_conf.c
|
|
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-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..261c965 100644
|
|
--- a/src/lcec_conf.h
|
|
+++ b/src/lcec_conf.h
|
|
@@ -55,6 +55,7 @@ typedef enum {
|
|
lcecPdoEntTypeSimple,
|
|
lcecPdoEntTypeFloatSigned,
|
|
lcecPdoEntTypeFloatUnsigned,
|
|
+ lcecPdoEntTypeFloatDouble,
|
|
lcecPdoEntTypeComplex,
|
|
lcecPdoEntTypeFloatIeee
|
|
} LCEC_PDOENT_TYPE_T;
|
|
diff --git a/src/lcec_generic.c b/src/lcec_generic.c
|
|
index dfddf73..41a13a9 100644
|
|
--- a/src/lcec_generic.c
|
|
+++ b/src/lcec_generic.c
|
|
@@ -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 > 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 +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 == 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);
|
|
+ return;
|
|
+ }
|
|
+ union {
|
|
+ double d;
|
|
+ uint64_t u;
|
|
+ } v;
|
|
+ v.d = fval; // Make an equivalent long int for bit operations (don't work on doubles)
|
|
+ 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;
|
|
+ }
|
|
+}
|
|
+
|