Re: Support for piezo controller nanoFAKTUR EBD-060310

[LiangChih Chiang via Tech-talk <tech-talk at aps.anl.gov>]

Hello, EPICS mates,

I started to implement the “Model 3” driver for nanoFaktur EBD-060310 piezo controller.

The attached files(.cpp and .h) are the work-in-progress source code. Not finished yet.

I have some questions.

1.

In asynMotorController.h, the data type of motorPosition_ and motorEncoderPosition_ are ints.

But in derived class of asynMotorcontroller(like smarActMCSMotorDriver.cpp and ACRMotorDriver.cpp),

the function setDoubleParam is used to set the above parameters.

Why is that?

2.

I can read the position(unit um, range 0 to 40) and voltage(unit volts, range -30 to 130) from the controller.

But which one should I set to  motorPosition_and  motorEncoderPosition_?

I don't quite understand.

3.

The values of position and voltage from the controller are float values.

When setting, say 2.123456, to motorEncoderPosition, it will become 2 (which is an int).

How to handle this situation?

Should I do some kind of transformation? For example, multiply by 1000,000?

4.

In the member function: asynStatus poll(bool* moving_p),

I need to set the moving/done status of the stage to moving_p and motorStatusDone_.

But there is no such exact suitable command provided by thhe controller.

A possible candidate is "get on-target status" meaning in closed-loop mode(servo ON),

whether the position(from sensor) approached the set target or not.

5.

The controller has two modes: 

closed-loop mode(servo ON), users set the desired potion(unit um).

open-loop mode(servo OFF), uses set the voltage(unit volts).

What should I set to motorPosition_? 

I think it should be something like "motor pulse".

what should I set to motorEncoderPosition_.? 

I think it should be something like "encoder value".

ps

The controller uses binary communication protocol.

The following example is to read the position(in um) of channel 0:

hexadecimal bytes in little-endian:

10 00 01 20 02 00 20 00 00 ac 01 00 00 00 00 fe

10 00: total length, 16 bytes

01 20: command ID, 0x2001 means "get position"

02 00: custom ID, used to distinguish different client programs.

20: option, 0x20 means "read"

00: sequence number, Used in very long responses. Not used in commands.

00: interface id, RS-232 or USB or Ethernet. Not used in commands.

ac: header checksum 

01: data format, 01 means 32-bit unsigned int

00 00 00 00: data, channel 0

fe: checksum

The  response:

13 00 01 20 02 00 10 00 01 b8 00 00 02 70 74 6a 3e 

13 00: total length, 19 bytes

01 20: command ID, 0x2001 means "get position"

02 00: custom ID, used to distinguish different client programs.

10: option, 0x10 means "final, no more".

00: sequence number,

01: interface id, 0x01 means RS-232.

b8: header checksum 

00: data format, 00 means 8-bit unsigned int

00: data, channel 0

02: data format, 02 means float

70 74 6a 3e: data, 0.228960 (position in um)

3e: checksum

Any suggestions and responses are appreciated.

Thank you.

On Thu, Apr 24, 2025 at 5:31 AM Mark Rivers <rivers at cars.uchicago.edu> wrote:

• I think I would need to write the EPICS motor driver module for it.

 

That is correct.  You should write a “Model 3” driver, meaning it is a C++ driver derived from the base classes asynMotorController and asynMotorAxis.

 

I just looked at the manual for the nanoFAKTUR EBD-060310.  It uses a non-standard binary communications protocol, not ASCII strings.  That is unusual, most controllers use ASCII communication.

 

The driver you suggested as a model, https://github.com/epics-motor/motorPIGCS2  is fairly complex because of the class hierarchy, so you might want to start with something simpler as your example. 

 

https://github.com/epics-motor/motorSmarAct/blob/master/smarActApp/src/smarActMCSMotorDriver.cpp

 

That uses ASCII communication, but it should not be too hard to take the concepts and use binary communication.

 

Mark

 

 

 

 

 

From: Tech-talk <tech-talk-bounces at aps.anl.gov> On Behalf Of LiangChih Chiang via Tech-talk
Sent: Wednesday, April 23, 2025 1:41 AM
To: tech-talk at aps.anl.gov
Subject: Support for piezo controller nanoFAKTUR EBD-060310

 

Hello, EPICS mates,

 

I need to control a piezo sage with the controller nanoFAKTUR EBD-060310.

This is my first time using this controller.

I searched and couldn't find the EPICS support module for it.

 

I think I would need to write the EPICS motor driver module for it.

 

What module code would you recommend me to use as a starter reference?

For example, something like

EPICS motor drivers for Physik Instrumente GCS2 (General Command Set) compatible controllers

https://github.com/epics-motor/motorPIGCS2

 

 

/* Motor driver support for nanoFAKTUR EBD-060310 Piezo Controller */

/* Derived from ACRMotorDriver.cpp by Mark Rivers, 2011/3/28 */
/* Derived from smarActMCSMotorDriver.cpp Till Straumann <strauman at slac.stanford.edu>, 9/11 */

/* Author: LiangChih Chiang <chiang.lc at nsrrc.org.tw>, 2026/01/21 */

#include <nanoFakturEBDDriver.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <iocsh.h>
#include <epicsString.h>
#include <epicsExport.h>
#include <asynOctetSyncIO.h>
#include <errlog.h>

/* Static configuration parameters (compile-time constants) */
// #undef  DEBUG
#define DEBUG

/* The asyn motor driver apparently can't cope with exceptions */
#undef  ASYN_CANDO_EXCEPTIONS
// #define ASYN_CANDO_EXCEPTIONS

/* Define this if exceptions should be thrown and it is OK to abort the application */
#undef  DO_THROW_EXCEPTIONS
// #define DO_THROW_EXCEPTIONS

#if defined(ASYN_CANDO_EXCEPTIONS) || defined(DO_THROW_EXCEPTIONS)
#define THROW_(e) throw e
#else
#define THROW_(e) epicsPrintf("%s\n",e.what());
#endif

#define DEFAULT_TIMEOUT 2.0


/* endian, byte order; start */

#define FLOAT_SIZE 4
#define DOUBLE_SIZE 8

// determine the endian of the platform
static int is_little_endian() {
    uint32_t test = 1;
    return *(uint8_t*)&test == 1;
}

// reverse byte order
static void reverse_bytes(uint8_t* bytes, size_t len) {
    size_t i;
    for (i = 0; i < len / 2; i++) {
        uint8_t temp = bytes[i];
        bytes[i] = bytes[len - 1 - i];
        bytes[len - 1 - i] = temp;
    }
}

// little-endian 4-bytes to float(of the platform-endian)
static float le_bytes_to_float(const uint8_t* le_bytes) {
    uint8_t temp[FLOAT_SIZE];
    float result;
    
    memcpy(temp, le_bytes, FLOAT_SIZE);  // copy to a temporary buffer
    
    if (!is_little_endian()) {  // reverse byte order if it's big-endian
        reverse_bytes(temp, FLOAT_SIZE);
    }
    
    memcpy(&result, temp, FLOAT_SIZE);  // platform-endian
    
    return result;
}

// float(of the platform-endian) to little-endian 4-bytes
static void float_to_le_bytes(float value, uint8_t* le_bytes) {
    uint8_t temp[FLOAT_SIZE];
    
    memcpy(temp, &value, FLOAT_SIZE);
    
    if (!is_little_endian()) {
        reverse_bytes(temp, FLOAT_SIZE);
    }
    
    memcpy(le_bytes, temp, FLOAT_SIZE);
}


// little-endian 8-bytes to double(of the platform-endian)
static double le_bytes_to_double(const uint8_t* le_bytes) {
    uint8_t temp[DOUBLE_SIZE];
    double result;
    
    memcpy(temp, le_bytes, DOUBLE_SIZE);
    
    if (!is_little_endian()) {
        reverse_bytes(temp, DOUBLE_SIZE);
    }
    
    memcpy(&result, temp, DOUBLE_SIZE);
    
    return result;
}

// double(of the platform-endian) to little-endian 8-bytes
static void double_to_le_bytes(double value, uint8_t* le_bytes) {
    uint8_t temp[DOUBLE_SIZE];
    
    memcpy(temp, &value, DOUBLE_SIZE);
    
    if (!is_little_endian()) {
        reverse_bytes(temp, DOUBLE_SIZE);
    }
    
    memcpy(le_bytes, temp, DOUBLE_SIZE);
}

/* endian, byte order; end */

/* nanaFaktur EBD; start */

// command id; consult nankFaktur EBD controller manual for details
#define CMD_CMD_LEVEL                0xFFF0

#define CMD_STOP_MOTION              0x2043
#define CMD_SERVO_ON                 0x2040

#define CMD_CURRENT_CTRL_TARGET      0x2015
#define CMD_CURRENT_CTRL_VOLT        0x2014
#define CMD_CURRENT_POS_ERROR        0x2013
#define CMD_CURRENT_TARGET           0x2012
#define CMD_OVERFLOW_STATUS          0x2011
#define CMD_ON_TARGET_STATUS         0x2010
#define CMD_OPEN_LOOP_TARGET_REL     0x2005
#define CMD_OPEN_LOOP_TARGET         0x2004
#define CMD_CLOSE_LOOP_TARGET_REL    0x2003
#define CMD_CLOSE_LOOP_TARGET        0x2002
#define CMD_GET_POS                  0x2001

// command and response use the same binary package definition; 
// byte order is little-endian;
// a package contains a header and zero-to-multiple parameters(data)
//
// uint16_t    len;          // total package size
// uint16_t    CmdId;        // command ID
// uint16_t    CustomId;     // value will be just returned (can be used for software-routinue)
// uint8_t     opt;          // option: e.g. controller should acknowledge, CRC or check-sum, etc.
// uint8_t     seq;          // sequence number: set by controller for response
// uint8_t     IntfId;       // interface ID: set by controller
// uint8_t     ChkSumHeader; // checksum for the header
// uint8_t     param0_fmt;   // parameters
//             param0_data   // size of data depends on the format
// uint8_t     param1_fmt;
//             param1_data
//             etc..
// uint8_t     ChkSumData;   // checksum for parameters

// from nF_common.h
#define MAX_PACKAGE_LEN                 1024 //!< maximal comamnd/response package size
#define MAX_CMDIN_PARAMETER_NUM          128 //!< maximal number of parameters in command package (to controller)
#define MAX_RSP_PARAMETER_NUM            512 //!< maximal number of parameters in response package (from controller)
#define MAX_PARAMETER_STRLEN              32 //!< maximal string length for controller's paramaters

// define lesser value which is ok if no need to process a response
// containing a lot of strings
#define MAX_PARAM_COUNT                    6 

// parameters union; modified from nF_interface.h
union ParamVal{ 
    double    dData;     // double
    float     fData;     // float
    uint32_t  uData;     // unsigned int
    int32_t   nData;     // signed int 
    char      sData[MAX_PARAMETER_STRLEN]; // space reserved for string
};
                            
// package header
#define HEADER_AND_CHECKSUM_SIZE (10)
#define HEADER_SIZE (HEADER_AND_CHECKSUM_SIZE - 1)
#define HEADER_START_I (0)
#define HEADER_CHECKSUM_I (HEADER_START_I + HEADER_SIZE)

// package params(data)
// params_and_checksum_size = package_size - HEADER_AND_CHECKSUM_SIZE
// params_size = params_and_checksum_size - 1
#define PARAMS_START_I HEADER_AND_CHECKSUM_SIZE
// params_checksum_i = package_size - 1

// from nF_common.h; params(data) format
#define CMD_DATA_FMT_U8                 0   // unsigned char (Byte)
#define CMD_DATA_FMT_U32                1   // unsigned int (D-Word)
#define CMD_DATA_FMT_FLOAT              2   // float
#define CMD_DATA_FMT_DOUBLE             3   // double
#define CMD_DATA_FMT_STRING             4   // string
#define CMD_DATA_FMT_S32                5   // int
#define CMD_DATA_FMT_CHAR               6   // char
#define CMD_DATA_FMT_U16                7   // unsigned short (Word)
#define CMD_DATA_FMT_S16                8   // short
#define CMD_DATA_FMT_ARRAY              9   // array of data: (fmt_array, ArrSize, dataFmt, data[])
#define CMD_DATA_FMT_LF                 10  // for host software: show a line-feed
#define CMD_DATA_FMT_INVALID            0xFF

typedef struct {
    uint16_t  len;
    uint16_t  cmdId; 
    uint16_t  customId;
    uint8_t   opt;    
    uint8_t   seq;
    uint8_t   intfId;
    uint16_t  checksumHeader;
    size_t    paramsCount;              // not in actual package
    uint8_t   fmts[MAX_PARAM_COUNT];    // parameter formats
    ParamVal  vals[MAX_PARAM_COUNT];    // parameter values
    uint16_t  checksumParams;
} nfPackageStruct;

// note: nFControl.exe uses 0x0001
//       Command Terminal in nFControl.exe uses 0x0002
#define CUSTOM_ID_EPICS 0x000E 

#define OPT_CMD_READ       0x20
#define OPT_CMD_WRITE      0x21

#define OPT_RSP_READ_MORE  0x90
#define OPT_RSP_READ_END   0x10

#define OPT_RSP_READ_ERROR 0x20

// ok or error
#define OPT_RSP_WRITE 0x21

// error code of response
// 0(zero) is ok; other values are errors
// as I know, error codes are all negative values.
#define RSP_ERR_OK 0

static const char *rsp_err_to_str(int32_t err) {
    switch (err) {
        case RSP_ERR_OK: return "no error";
        case -1:  return "internal system error";
        case -2:  return "internal memory error";
        case -3:  return "internal function error";
        case -10: return "command package length error";
        case -11: return "command package check-sum error";
        case -12: return "command input timeout error";
        case -20: return "unknown command";
        case -21: return "command operation not allowed";
        case -22: return "currently command level too low";
        case -23: return "command input syntax error";
        case -24: return "command index out-of-range";
        case -25: return "command input-data-format error";  
        case -26: return "command input-data-value out-of-range";
        case -27: return "invalid number of input-data";
        case -28: return "command execution timeout";
        case -30: return "unknown parameter";            
        case -31: return "parameter operation not allowed";
        case -32: return "parameter needs higher command level";
        case -33: return "parameter input syntax error";
        case -34: return "parameter index out-of-range";
        case -35: return "parameter data-format error";
        case -36: return "parameter value out-of-range";
        case -40: return "wave parameter not valid";
        case -37: return "string length out-of-range"; 
        case -50: return "command password error";
        case -51: return "command/parameter is read-only";
        case -52: return "internal no enough memory error";
        case -70: return "macro not found";
        case -71: return "macro open() error";
        case -72: return "macro read() error";
        case -73: return "macro write() error";
        case -74: return "macro remove() error";
        case -75: return "macro is running";
        case -76: return "not allowed when receiving macro";
        default:  return "Unknown response error code";
    }
}

// error code of parsing packages
typedef enum {
    PKG_ERR_OK = 0,
    PKG_ERR_LEN = -1,
    PKG_ERR_HEADER_CHECKSUM = -2,
    PKG_ERR_PARAMS_CHECKSUM = -3,
    PKG_ERR_PARAMS_TYPE = -4,
} nfPkgErrCode;

static const char *pkg_err_to_str(nfPkgErrCode err) {
    switch (err) {
        case PKG_ERR_OK:              return "Ok";
        case PKG_ERR_LEN:             return "Package length(size) error";
        case PKG_ERR_HEADER_CHECKSUM: return "Header checksum error";
        case PKG_ERR_PARAMS_CHECKSUM: return "Params checksum error";
        case PKG_ERR_PARAMS_TYPE:     return "Params data type error";
        default:                      return "Unknown error code";
    }
}

// calculate the checksum of package header and package params
static uint8_t checksum(const uint8_t *arr, size_t len){
    size_t i;
    uint8_t cks = 0;
    for(i = 0; i < len; i++){
        cks += arr[i];
    }
    cks = 0xFF - cks;
    return cks;
}

// parse package bytes into nfPackageStruct
static nfPkgErrCode parse_pkg(uint8_t *pkg, size_t pkg_size, nfPackageStruct *ps){   
    nfPkgErrCode err = PKG_ERR_OK;
    
    ps->len = pkg[0] | (pkg[1] << 8);
    if(ps->len != pkg_size){
#ifdef DEBUG
        printf("Error: package size check fail: in package %u, given size %lu\n", ps->len, pkg_size); 
#endif
        err = PKG_ERR_LEN;
        return err;
    }

    ps->cmdId = pkg[2] | (pkg[3] << 8);
    ps->customId = pkg[4] | (pkg[5] << 8);
    ps->opt = pkg[6];
    ps->seq = pkg[7];
    ps->intfId = pkg[8];
    
    ps->checksumHeader = pkg[HEADER_CHECKSUM_I];
    const uint8_t checksumHeaderCalc = checksum(pkg, HEADER_SIZE);
    if(ps->checksumHeader != checksumHeaderCalc){
#ifdef DEBUG
        printf("Error: header checksum fail: in package 0x%02x, calculated 0x%02x.\n", ps->checksumHeader, checksumHeaderCalc);
#endif
        err = PKG_ERR_HEADER_CHECKSUM;
        return err;
    }

    // parameters
    size_t i = PARAMS_START_I;  // index into package bytes
    const size_t params_checksum_i = pkg_size - 1;
   
    size_t j = 0;  // index into params

    while(i < params_checksum_i){
        ps->fmts[j] = pkg[i];
        
        switch(pkg[i]){
            case CMD_DATA_FMT_U8:
                ps->vals[j].uData = pkg[i+1];
                i += (1+1);
            break;
            
            case CMD_DATA_FMT_U32:
                ps->vals[j].uData = pkg[i+1] | (pkg[i+2] << 8) | (pkg[i+3] << 16) | (pkg[i+4] << 24);
                i += (1+4);
            break;

            case CMD_DATA_FMT_FLOAT:
                memcpy(&ps->vals[j].fData, &pkg[i+1], FLOAT_SIZE);
                i += (1+FLOAT_SIZE);
            break;

            case CMD_DATA_FMT_DOUBLE: 
                memcpy(&ps->vals[j].dData, &pkg[i+1], DOUBLE_SIZE);
                i += (1+DOUBLE_SIZE);
            break;

            case CMD_DATA_FMT_STRING:
                strcpy(ps->vals[j].sData, (const char *)&pkg[i+1]);
                i += (1 + strlen((const char *)&pkg[i+1]) + 1);  // include NULL
            break;

            case CMD_DATA_FMT_S32:
                ps->vals[j].nData = pkg[i+1] | (pkg[i+2] << 8) | (pkg[i+3] << 16) | (pkg[i+4] << 24);
                i += (1+4);
            break;

            case CMD_DATA_FMT_CHAR:
                ps->vals[j].uData = pkg[i+1];
                i += (1+1);
            break;

            case CMD_DATA_FMT_U16:
                ps->vals[j].uData = pkg[i+1] | (pkg[i+2] << 8);
                i += (1+2);
            break;

            case CMD_DATA_FMT_S16:
                ps->vals[j].nData = pkg[i+1] | (pkg[i+2] << 8);
                i += (1+2);
            break;

            case CMD_DATA_FMT_ARRAY:  // can not handle; skip remaining params
                i = pkg_size;
#ifdef DEBUG
                printf("Warning: un-handled param data format: 0x%02x(CMD_DATA_FMT_ARRAY).\n", CMD_DATA_FMT_ARRAY);
#endif
            break;

            case CMD_DATA_FMT_LF:
                i += 1;
            break;

            case CMD_DATA_FMT_INVALID:  // can not handle; skip remaining params
                i = pkg_size;
#ifdef DEBUG
                printf("Warning: un-handled param data format: 0x%02x(CMD_DATA_FMT_INVALID).\n", CMD_DATA_FMT_INVALID);
#endif
            break;
            
            default:  // can not handle; skip remaining params
                i = pkg_size;
#ifdef DEBUG
                printf("Warning: un-handled unknown param data format: 0x%02x(?).\n", pkg[i]);
#endif
            break;
        }
        
        j++;
    }   
    ps->paramsCount = j;

    if(ps->paramsCount > 0){  // has params
        const size_t params_and_checksum_size = pkg_size - HEADER_AND_CHECKSUM_SIZE;
        const size_t params_size = params_and_checksum_size - 1;

        ps->checksumParams = pkg[params_checksum_i];
        const uint8_t checksumParamsCalc = checksum(&pkg[PARAMS_START_I], params_size);
        if(ps->checksumParams != checksumParamsCalc){
#ifdef DEBUG
            printf("Error: params checksum fail: in package 0x%02x, calculated 0x%02x.\n", ps->checksumParams, checksumParamsCalc);
#endif
            err = PKG_ERR_PARAMS_CHECKSUM;
            return err;
        }
    }

    return err;
}

#define NF_IS_WRITE_CMD 1
#define NF_IS_READ_CMD 0

// make command package
// return package size
static size_t make_pkg(uint16_t cmdId, int isReadCmd, size_t params_count, const uint8_t *fmts, const ParamVal *params, uint8_t *pkg){
    size_t i = PARAMS_START_I; // package index
    size_t j;                  // params count index
    
    for(j = 0; j < params_count; j++){  // params
        switch(fmts[j]){
            case CMD_DATA_FMT_U8:
                pkg[i] = CMD_DATA_FMT_U8;
                pkg[i+1] = params[j].uData & 0xFF;
                i += (1+1);
            break;
            
            case CMD_DATA_FMT_U32:
                pkg[i] = CMD_DATA_FMT_U32;
                pkg[i+1] = params[j].uData & 0xFF;
                pkg[i+2] = (params[j].uData >> 8) & 0xFF;
                pkg[i+3] = (params[j].uData >> 16) & 0xFF;
                pkg[i+4] = (params[j].uData >> 24) & 0xFF;
                i += (1+4);
            break;

            case CMD_DATA_FMT_FLOAT:
                pkg[i] = CMD_DATA_FMT_FLOAT;
                float_to_le_bytes(params[j].fData, &pkg[i+1]);
                i += (1+FLOAT_SIZE);
            break;

            case CMD_DATA_FMT_DOUBLE:
                pkg[i] = CMD_DATA_FMT_DOUBLE;
                double_to_le_bytes(params[j].dData, &pkg[i+1]);
                i += (1+DOUBLE_SIZE);
            break;

            case CMD_DATA_FMT_STRING:
                pkg[i] = CMD_DATA_FMT_STRING;
                strcpy((char *)&pkg[i+1], params[j].sData);
                i += (1 + strlen((char *)&pkg[i+1]) + 1);
            break;

            case CMD_DATA_FMT_S32:
                pkg[i] = CMD_DATA_FMT_S32;
                pkg[i+1] = params[j].nData & 0xFF;
                pkg[i+2] = (params[j].nData >> 8) & 0xFF;
                pkg[i+3] = (params[j].nData >> 16) & 0xFF;
                pkg[i+4] = (params[j].nData >> 24) & 0xFF;
                i += (1+4);
            break;

            case CMD_DATA_FMT_CHAR:
                pkg[i] = CMD_DATA_FMT_CHAR;
                pkg[i+1] = params[j].uData & 0xFF;
                i += (1+1);
            break;

            case CMD_DATA_FMT_U16:
                pkg[i] = CMD_DATA_FMT_U16;
                pkg[i+1] = params[j].uData & 0xFF;
                pkg[i+2] = (params[j].uData >> 8) & 0xFF;
                i += (1+2);
            break;

            case CMD_DATA_FMT_S16:
                pkg[i] = CMD_DATA_FMT_S16;
                pkg[i+1] = params[j].nData & 0xFF;
                pkg[i+2] = (params[j].nData >> 8) & 0xFF;
                i += (1+2);
            break;

            case CMD_DATA_FMT_ARRAY:  // can not handle; skip remaining params
                j = params_count;
#ifdef DEBUG
                printf("Warning: un-handled param data format: 0x%02x(CMD_DATA_FMT_ARRAY).\n", CMD_DATA_FMT_ARRAY);
#endif
            break;

            case CMD_DATA_FMT_LF:
                pkg[i] = CMD_DATA_FMT_LF;
                i += (1);
            break;

            case CMD_DATA_FMT_INVALID:  // can not handle; skip remaining params
                j = params_count;
#ifdef DEBUG
                printf("Warning: un-handled param data format: 0x%02x(CMD_DATA_FMT_INVALID).\n", CMD_DATA_FMT_INVALID);
#endif
            break;
            
            default:  // can not handle; skip remaining params
                j = params_count;
#ifdef DEBUG
                printf("Warning: un-handled unknown param data format: 0x%02x(?).\n", fmts[j]);
#endif
            break;

        }
    }
    
    const uint16_t params_size = i - PARAMS_START_I;  // if 0, means no params
    const uint16_t params_checksum_size = params_size > 0 ? params_size + 1 : 0;
    const uint16_t pkg_size = HEADER_AND_CHECKSUM_SIZE + params_checksum_size;
    
    pkg[0] = pkg_size & 0xFF;                 // len(size)
    pkg[1] = (pkg_size >> 8) & 0xFF;

    pkg[2] = cmdId & 0xFF;                    // cmdId
    pkg[3] = (cmdId >> 8) & 0xFF;
    
    pkg[4] = CUSTOM_ID_EPICS & 0xFF;          // customId
    pkg[5] = (CUSTOM_ID_EPICS >> 8) & 0xFF;
    
    pkg[6] = isReadCmd == NF_IS_READ_CMD ? OPT_CMD_READ : OPT_CMD_WRITE;  // opt
    
    pkg[7] = 0;  // seq
    pkg[8] = 0;  // intfId

    pkg[HEADER_CHECKSUM_I] = checksum(pkg, HEADER_SIZE);  // header checksum

    if(params_size > 0){
        const uint16_t params_checksum_i = pkg_size - 1;
        pkg[params_checksum_i] = checksum(&pkg[PARAMS_START_I], params_size);  // params checksum
    }
    
    return pkg_size;
}

/* nanaFaktur EBD; end */

/* NanoFakturEBDException; start */ 

NanoFakturEBDException::NanoFakturEBDException(NanoFakturEBDExceptionType t, const char *fmt, ...)
	: t_(t)
{
    va_list ap;
	if ( fmt ) {
		va_start(ap, fmt);
		epicsVsnprintf(str_, sizeof(str_), fmt, ap);
		va_end(ap);
	} else {
		str_[0] = 0;
	}
};

NanoFakturEBDException::NanoFakturEBDException(NanoFakturEBDExceptionType t, const char *fmt, va_list ap)
    : t_(t)
{
	epicsVsnprintf(str_, sizeof(str_), fmt, ap);
}

/* NanoFakturEBDException; end */ 

/* NanoFakturEBDController; start */ 

NanoFakturEBDController::NanoFakturEBDController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod)
	: asynMotorController(portName, // portName
                          numAxes,  // numAxes
	                      0,        // numParams
	                      0,        // interfaceMask
	                      0,        // interruptMask
	                      ASYN_CANBLOCK | ASYN_MULTIDEVICE, // asynFlags
	                      1,        // autoconnect
	                      0,        // priority
                          0)        // stack size
	, asynUserMot_p_(0)
{
    asynStatus       status;
    char             junk[100];
    size_t           got_junk;
    int              eomReason;
    pAxes_ = (NanoFakturEBDAxis **)(asynMotorController::pAxes_);

    asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, "NanoFakturEBDController pasynOctetSyncIO->connect to port %s\n", portName);

    /* asynStatus (*connect)(const char *port, int addr, asynUser **ppasynUser, const char *drvInfo); */
	status = pasynOctetSyncIO->connect(IOPortName, 0, &asynUserMot_p_, NULL);

	if(status){
		asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
		          "NanoFakturEBDController pasynOctetSyncIO->connect: cannot connect to port %s\n",
                  portName);
		THROW_(NanoFakturEBDException(EBDConnectionError, "NanoFakturEBDController: unable to connect to controller"));
	}

	// slurp away any initial telnet negotiation; there is no guarantee that
	// the other end will not send some telnet chars in the future. The terminal
	// server should really be configured to 'raw' mode!
	status = pasynOctetSyncIO->read(asynUserMot_p_, junk, sizeof(junk), 0.5, &got_junk, &eomReason);
	if(got_junk){
		asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING, "NanoFakturEBDController port %s: Warning: detected unexpected bytes on link (%s) initially.\n", portName, IOPortName);
	}

    /* set command level of the controller to 1; default is 0, not high enough */
    status = setUint(CMD_CMD_LEVEL, 1);
	if(status){
		asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
		          "NanoFakturEBDController port %s: Error: set command level failed. \n", portName);
	}

    // nanaFaktur EBD controllers use binary protocol, not ASCII protocol.
    // pasynOctetSyncIO->setInputEos(asynUserMot_p_, "\n", 1);
    // pasynOctetSyncIO->setOutputEos(asynUserMot_p_, "\n", 1);


	// Create axes; move to iocsh function nanoFakturEBDCreateAxis()
/*	for(ax = 0; ax <  numAxes; ax++){
		pAxes_[ax] = new NanoFakturEBDAxis(this, ax);
	}
*/

	startPoller(movingPollPeriod, idlePollPeriod, 2);
}


// read command,  channel
//      response, ok, channel value     u8 u32 float
//      response, error code s32 < 0
//
// write command, channel value(uint or float)
//       response, ok, s32 0            Acknowledge
//       response, error code s32 < 0   


// readInt, readFloat, return ok_or_error
//                     if something wrong, print
// write fmt value  int, float

//            read                  write
// 0x2001     u32 index             -                         get position
//            u8 index, float v
// 0x2002     u32 index             u32 index, float v        close-loop target
//            u8 index, float v     s32 0 ack
// 0x2003     -                     u32 index, float v        set close-loop target relative 
//                                  s32 0 ack
// 0x2010     u32 index             -                         get on-target status 
//            u8 index, u8 v(0,1)
// 0x2011     u32 index             -                         get overflow status 
//            u8 index, u8 v(0,1)
// 0x2040     u32 index             u32 index, u32 v(0,1)     servo controlling set/get 
//            u8 index, u8 v(0,1)   s32 0 ack
//
// 0x2043     -                     u32 index, (no value)     Stop motion 
//                                  s32 0 ack
//
// 0xFFF0     -                     u32 level                 command level


// send a command and get response
asynStatus NanoFakturEBDController::cmdRsp(uint16_t cmdId, int32_t isReadCmd, size_t params_count, const uint8_t *fmts, const ParamVal *vals, size_t *params_count_out, uint8_t *fmts_out, ParamVal *vals_out)
{
    asynStatus status;
    uint8_t    pkg[MAX_PACKAGE_LEN];
    size_t     pkg_size;
    size_t     nwrite;
    int        eomReason;
    uint8_t    pkg_rsp[MAX_PACKAGE_LEN];
    size_t     pkg_rsp_size;
    nfPackageStruct ps_rsp;
    nfPkgErrCode pkg_rsp_err_code;
    size_t     j;

    pkg_size = make_pkg(cmdId, isReadCmd, params_count, fmts, vals, pkg);

/*
asynStatus (*writeRead)(asynUser   *pasynUser,
                        const char *write_buffer, size_t write_buffer_len,
                        char       *read_buffer,  size_t read_buffer_len,
                        double      timeout, 
                        size_t     *nbytesOut,    size_t *nbytesIn, 
                        int        *eomReason);
*/                
	status = pasynOctetSyncIO->writeRead(asynUserMot_p_, (const char *)pkg, pkg_size, 
                                         (char *)pkg_rsp, MAX_PACKAGE_LEN, 
                                         DEFAULT_TIMEOUT, &nwrite, &pkg_rsp_size, &eomReason);
    
    if(asynSuccess != status){
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
	              "Error: NanoFakturEBDController::cmdRsp pasynOctetSyncIO->writeRead, cmdId %04x, asynStatus %d\n",
                  cmdId, status);

        return status;
    }
    
    pkg_rsp_err_code = parse_pkg(pkg_rsp, pkg_rsp_size, &ps_rsp);
    if(PKG_ERR_OK != pkg_rsp_err_code){
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
	              "Error: NanoFakturEBDController::cmdRsp response package parse error, cmdId %04x, pkg_rsp_err_code %d, %s\n",
                  cmdId, pkg_rsp_err_code, pkg_err_to_str(pkg_rsp_err_code));

        status = asynError;
        return status;
    }
    
    // check if error 
    if(ps_rsp.paramsCount == 1 && ps_rsp.fmts[0] == CMD_DATA_FMT_S32 && ps_rsp.vals[0].nData != 0){
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
                  "Error: cmdRsp response error, cmdId 0x%04x, rsp_err_code %d %s\n",
                  cmdId, ps_rsp.vals[0].nData, rsp_err_to_str(ps_rsp.vals[0].nData));
        status = asynError;
        return status;
    }

    *params_count_out = ps_rsp.paramsCount;
    for(j = 0; j < ps_rsp.paramsCount; j++){
        fmts_out[j] = ps_rsp.fmts[j];
        vals_out[j] = ps_rsp.vals[j];
    }
    
    return status;
}

// read command to get a value. 
// fmt0/val0 is ususally channel number; can be CMD_DATA_FMT_INVALID if no params
asynStatus NanoFakturEBDController::getVal(uint16_t cmdId, uint8_t fmt0, const void *val0, void *val_out)
{
    asynStatus status;
    size_t     params_count = 0;
    uint8_t    fmts[1];
    ParamVal   vals[1];
    size_t     params_count_out;
    uint8_t    fmts_out[3];  // channel, val_out, 
    ParamVal   vals_out[3];  // params returned from controller may contains an LF

    switch(fmt0){
        case CMD_DATA_FMT_U8:
        case CMD_DATA_FMT_U16:
        case CMD_DATA_FMT_U32:
            fmts[0] = fmt0;
            vals[0].uData = *(const uint32_t *)val0;
            params_count++;
        break;

        case CMD_DATA_FMT_CHAR:
        case CMD_DATA_FMT_S16:
        case CMD_DATA_FMT_S32:
            fmts[0] = fmt0;
            vals[0].nData = *(const int32_t *)val0;
            params_count++;
        break;

        case CMD_DATA_FMT_FLOAT:
        case CMD_DATA_FMT_DOUBLE:
            fmts[0] = fmt0;
            vals[0].fData = *(const float *)val0;
            params_count++;
        break;
        
        default:  // other formats are not handled
        break;
    }

    status = cmdRsp(cmdId, NF_IS_READ_CMD, params_count, fmts, vals, &params_count_out, fmts_out, vals_out);
    
    if(asynSuccess != status){
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
	              "Error: NanoFakturEBDController getVal cmdId %04x, asynStatus %d\n", cmdId, status);
        return status;
    }
                                              // assume [0] is channel number
    switch(fmts_out[1]){                     // magic number 1 is for val_out
        case CMD_DATA_FMT_U8:
        case CMD_DATA_FMT_U16:
        case CMD_DATA_FMT_U32:
            *(uint32_t *)val_out = vals_out[1].uData;
        break;

        case CMD_DATA_FMT_CHAR:
        case CMD_DATA_FMT_S16:
        case CMD_DATA_FMT_S32:
            *(int32_t *)val_out = vals_out[1].nData;
        break;

        case CMD_DATA_FMT_FLOAT:
        case CMD_DATA_FMT_DOUBLE:
            *(float *)val_out = vals_out[1].fData;
        break;

    }

    return status;
}

// read command; no params(no channel)
// example: 
asynStatus NanoFakturEBDController::getUint(uint16_t cmdId, uint32_t *val_out)
{
    return getVal(cmdId, CMD_DATA_FMT_INVALID, NULL, val_out);
}
asynStatus NanoFakturEBDController::getInt(uint16_t cmdId, int32_t *val_out)
{
    return getVal(cmdId, CMD_DATA_FMT_INVALID, NULL, val_out);
}
asynStatus NanoFakturEBDController::getFloat(uint16_t cmdId, float *val_out)
{
    return getVal(cmdId, CMD_DATA_FMT_INVALID, NULL, val_out);
}

// write command
// fmt0/val0 is ususally channel number or the value to set
// fmt1/val1 is usually the vale to set; can be CMD_DATA_FMT_INVALID if no params
// example: set closed-loop target of a channel
// example: set command level
asynStatus NanoFakturEBDController::setVal(uint16_t cmdId, uint8_t fmt0, const void *val0, uint8_t fmt1, const void *val1)
{
    asynStatus status;
    size_t     params_count = 0;
    uint8_t    fmts[2] = {fmt0, fmt1};        // magic number 2
    const void *p_vals[2] = {val0, val1};
    ParamVal   vals[2];
    size_t     params_count_out;
    size_t     j;
    uint8_t    fmts_out[2];       // index 0 for response error code, s32 0 is ok.
    ParamVal   vals_out[2];       // index 1 is for extra safety


    for(j = 0; j < 2; j++){
        switch(fmts[params_count]){
            case CMD_DATA_FMT_U8:
            case CMD_DATA_FMT_U16:
            case CMD_DATA_FMT_U32:
                vals[params_count].uData = *(const uint32_t *)p_vals[params_count];
                params_count++;
            break;

            case CMD_DATA_FMT_CHAR:
            case CMD_DATA_FMT_S16:
            case CMD_DATA_FMT_S32:
                vals[params_count].nData = *(const int32_t *)p_vals[params_count];
                params_count++;
            break;

            case CMD_DATA_FMT_FLOAT:
            case CMD_DATA_FMT_DOUBLE:
                vals[params_count].fData = *(const float *)p_vals[params_count];
                params_count++;
            break;
        }
    }
    
    status = cmdRsp(cmdId, NF_IS_WRITE_CMD, params_count, fmts, vals, &params_count_out, fmts_out, vals_out);

    if(asynSuccess != status){
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
	              "Error: NanoFakturEBDController setVal cmdId 0x%04x, asynStatus %d\n", cmdId, status);
        return status;
    }

    return status;
}

// write command; one param(value) to set
asynStatus NanoFakturEBDController::setUint(uint16_t cmdId, uint32_t val)
{
    return setVal(cmdId, CMD_DATA_FMT_U32, &val, CMD_DATA_FMT_INVALID, NULL);
}
asynStatus NanoFakturEBDController::setInt(uint16_t cmdId, int32_t val)
{
    return setVal(cmdId, CMD_DATA_FMT_S32, &val, CMD_DATA_FMT_INVALID, NULL);
}
asynStatus NanoFakturEBDController::setFloat(uint16_t cmdId, float val)
{
    return setVal(cmdId, CMD_DATA_FMT_FLOAT, &val, CMD_DATA_FMT_INVALID, NULL);
}

/* NanoFakturEBDController; end */ 

/* NanoFakturEBDAxis; start */ 

NanoFakturEBDAxis::NanoFakturEBDAxis(class NanoFakturEBDController *cnt_p, int axisNumber, int channel)
	: asynMotorAxis(cnt_p, axisNumber), c_p_(cnt_p)
{
	channel_ = channel;
    
	asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "NanoFakturEBDAxis -- creating axis %d, channel %d\n", axisNumber, channel);

    setIntegerParam(c_p_->motorStatusHasEncoder_, 1);
    setIntegerParam(c_p_->motorStatusGainSupport_, 1);
    
    // setIntegerParam(c_p_->motorPowerAutoOnOff_, 0);
    // setDoubleParam(c_p_->motorPowerOnDelay_, 0);
    // setDoubleParam(c_p_->motorPowerOffDelay_, 0);
    // setDoubleParam(c_p_->motorPostMoveDelay_, 0);


    // turn on servo; closed-loop
    if( (comStatus_ = setUint(CMD_SERVO_ON, 1)) ){
        setIntegerParam(c_p_->motorClosedLoop_, 0);
        goto bail;
    }
    setIntegerParam(c_p_->motorClosedLoop_, 1);

bail:
	setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0);
	setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0);

	callParamCallbacks();  /* Do callbacks so higher layers see any changes */

	if(comStatus_){
		THROW_(NanoFakturEBDException(EBDCommunicationError, "NanoFakturEBDAxis::NanoFakturEBDAxis -- channel %u, ASYN error %i", axisNumber, comStatus_));
	}
}

/* Obtain value of the 'motorClosedLoop_' parameter (which maps to the record's CNEN field) */
bool NanoFakturEBDAxis::getClosedLoop()
{
    int val;
	c_p_->getIntegerParam(channel_, c_p_->motorClosedLoop_, &val);
	return val;
}

/* return 1 if encoder exists; return 0 otherwise */
bool NanoFakturEBDAxis::hasEncoder()
{
	int val;
	c_p_->getIntegerParam(channel_, c_p_->motorStatusHasEncoder_, &val);
	return val;
}

// get(read) a value for this channel from controller (nothing to do with asyn's parameter library).
// read command, parameter 0 is channel number
// cmdId:   controller command id
// val_out: where to store the value returned by the controller
// return:  asynError if an error occurred, asynSuccess otherwise.
asynStatus NanoFakturEBDAxis::getUint(uint16_t cmdId, uint32_t *val_out)
{
    return c_p_->getVal(cmdId, CMD_DATA_FMT_U32, &channel_, val_out);
}
asynStatus NanoFakturEBDAxis::getInt(uint16_t cmdId, int32_t *val_out)
{
    return c_p_->getVal(cmdId, CMD_DATA_FMT_U32, &channel_, val_out);
}
asynStatus NanoFakturEBDAxis::getFloat(uint16_t cmdId, float *val_out)
{
    return c_p_->getVal(cmdId, CMD_DATA_FMT_U32, &channel_, val_out);
}

// set(write) a value for this channel to controller
// write command, parameter 0 is channel number, parameter 1 is the value to set
asynStatus NanoFakturEBDAxis::setUint(uint16_t cmdId, uint32_t val)
{
    return c_p_->setVal(cmdId, CMD_DATA_FMT_U32, &channel_, CMD_DATA_FMT_U32, &val);
}
asynStatus NanoFakturEBDAxis::setInt(uint16_t cmdId, int32_t val)
{
    return c_p_->setVal(cmdId, CMD_DATA_FMT_U32, &channel_, CMD_DATA_FMT_S32, &val);
}
asynStatus NanoFakturEBDAxis::setFloat(uint16_t cmdId, float val)
{
    return c_p_->setVal(cmdId, CMD_DATA_FMT_U32, &channel_, CMD_DATA_FMT_FLOAT, &val);
}
// write command, parameter 0 is channel number, no parameter 1
// example: stop motion
asynStatus NanoFakturEBDAxis::setNone(uint16_t cmdId)
{
    return c_p_->setVal(cmdId, CMD_DATA_FMT_U32, &channel_, CMD_DATA_FMT_INVALID, NULL);
}

asynStatus NanoFakturEBDAxis::poll(bool* moving_p)
{
	float    val_pos;
    float    val_target; // voltage or position
    uint32_t val_ontarget;
    uint32_t val_overflow;
    uint32_t cmdId = getClosedLoop() ? CMD_GET_POS : CMD_CURRENT_CTRL_TARGET;
    
    if( (comStatus_ = getFloat(CMD_GET_POS, &val_pos)) ){
        goto bail;
    }
	setDoubleParam(c_p_->motorEncoderPosition_, val_pos);
    
    if( (comStatus_ = getFloat(cmdId, &val_target)) ){
        goto bail;
    }
	setDoubleParam(c_p_->motorPosition_, val_target);
    
#ifdef DEBUG
	printf("NanoFakturEBDAxis poll, channel %d, position %f, target %f", channel_, val_pos, val_target);
#endif

    // is this correct?  there is no direct command for the moving/stopped status of axis
    if((comStatus_ = getUint(CMD_ON_TARGET_STATUS, &val_ontarget))){
		goto bail;
    }
    
    *moving_p = !(getClosedLoop() && (bool)val_ontarget);
	setIntegerParam(c_p_->motorStatusDone_, *moving_p ? 0 : 1);

#ifdef DEBUG
	printf(", on-target status %u", val_ontarget);
#endif

    if((comStatus_ = getUint(CMD_OVERFLOW_STATUS, &val_overflow))){
		goto bail;
    }
    setIntegerParam(c_p_->motorStatusProblem_, val_overflow ? 1 : 0);

#ifdef DEBUG
	printf(", overflow status %u", val_overflow);
#endif

bail:
	setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0);
	setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0);

#ifdef DEBUG
	printf(".\n");
#endif

	callParamCallbacks();  /* Do callbacks so higher layers see any changes */

	return comStatus_;
}


// unit of position, pulse
// unit of minVelocity and maxVelocity, pulse/second
// unit of acceleration, pulse/second^2,   = (maxV - minV) / ACCL
asynStatus NanoFakturEBDAxis::move(double position, int relative, double min_vel, double max_vel, double accel)
{
    uint32_t cmdId;
    
    if(getClosedLoop()){
        cmdId = relative ? CMD_CLOSE_LOOP_TARGET_REL : CMD_CLOSE_LOOP_TARGET;
    }
    else{
        cmdId = relative ? CMD_OPEN_LOOP_TARGET_REL : CMD_OPEN_LOOP_TARGET;
    }
    
#ifdef DEBUG
    printf("NanoFakturEBDAxis move channel %u, %s, %s to %f (speed %f - %f); accel %f\n", 
           channel_, getClosedLoop() ? "ClosedLoop" : "OpenLoop", relative ? "relative" : "absolute", position, min_vel, max_vel, accel);
#endif

    if( (comStatus_ = setFloat(cmdId, position)) ){
        goto bail;
    }

bail:
	if(comStatus_){
		setIntegerParam(c_p_->motorStatusProblem_, 1);
		setIntegerParam(c_p_->motorStatusCommsError_, 1);
		callParamCallbacks();
	}
	return comStatus_;
}

// jog
// No command we an use directly. 
// Just move to a   very far target.
asynStatus NanoFakturEBDAxis::moveVelocity(double min_vel, double max_vel, double accel)
{
    double position = 40*1000000;  // magic number
    uint32_t cmdId = getClosedLoop() ? CMD_CLOSE_LOOP_TARGET : CMD_OPEN_LOOP_TARGET;
    
	if(max_vel < 0){
		position = -position;
	}

#ifdef DEBUG
	printf("NanoFakturEBDAxis moveVelocity channel %u, (%f - %f), accel %f\n", 
           channel_, min_vel, max_vel, accel);
#endif

    if( (comStatus_ = setFloat(cmdId, position)) ){
        goto bail;
    }

bail:
	if(comStatus_){
		setIntegerParam(c_p_->motorStatusProblem_, 1);
		setIntegerParam(c_p_->motorStatusCommsError_, 1);
		callParamCallbacks();
	}
	return comStatus_;
}

asynStatus NanoFakturEBDAxis::stop(double acceleration)
{
#ifdef DEBUG
	printf("NanoFakturEBDAxis stop channel %d\n", channel_);
#endif

    comStatus_ = setNone(CMD_STOP_MOTION);

	if ( comStatus_ ) {
		setIntegerParam(c_p_->motorStatusProblem_, 1);
		setIntegerParam(c_p_->motorStatusCommsError_, 1);
		callParamCallbacks();
	}
	return comStatus_;
}

/* NanoFakturEBDAxis; end */ 


/* iocsh wrapping and registration business (stolen from ACRMotorDriver.cpp) */
/* nanoFakturEBDCreateController called to create one nanoFaktur EBD controller */
static const iocshArg cc_a0 = {"Port name [string]",               iocshArgString};
static const iocshArg cc_a1 = {"I/O port name [string]",           iocshArgString};
static const iocshArg cc_a2 = {"Number of axes [int]",             iocshArgInt};
static const iocshArg cc_a3 = {"Moving poll period (ms) [double]", iocshArgDouble};
static const iocshArg cc_a4 = {"Idle poll period (ms) [double]",   iocshArgDouble};

static const iocshArg * const cc_as[] = {&cc_a0, &cc_a1, &cc_a2, &cc_a3, &cc_a4};

static const iocshFuncDef cc_def = {"nanoFakturEBDCreateController", sizeof(cc_as)/sizeof(cc_as[0]), cc_as};

extern "C" void *
nanoFakturEBDCreateController(
	const char *motorPortName,    // port name used in motor record
	const char *ioPortName,       // asyn port name
	int         numAxes,
	double      movingPollPeriod,
	double      idlePollPeriod)
{
    void *rval = 0;
    
	// the asyn stuff doesn't seem to be prepared for exceptions. I get segfaults
	// if constructing a controller (or axis) incurs an exception even if its
	// caught (IMHO asyn should behave as if the controller/axis never existed...)
#ifdef ASYN_CANDO_EXCEPTIONS
	try {
#endif
		rval = new NanoFakturEBDController(motorPortName, ioPortName, numAxes, movingPollPeriod/1000, idlePollPeriod/1000);  // create controller
#ifdef ASYN_CANDO_EXCEPTIONS
	} catch (NanoFakturEBDException &e) {
		epicsPrintf("nanoFakturEBDCreateController failed (exception caught):\n%s\n", e.what());
		rval = 0;
	}
#endif

	return rval;
}

static void cc_fn(const iocshArgBuf *args)
{
	nanoFakturEBDCreateController(
		args[0].sval,
		args[1].sval,
		args[2].ival,
		args[3].dval,
		args[4].dval);
}


/* nanoFakturEBDCreateAxis called to create each axis of a nanoFaktur EBD controller */
static const iocshArg ca_a0 = {"Controller Port name [string]",    iocshArgString};
static const iocshArg ca_a1 = {"Axis number [int]",                iocshArgInt};
static const iocshArg ca_a2 = {"Channel [int]",                    iocshArgInt};

static const iocshArg * const ca_as[] = {&ca_a0, &ca_a1, &ca_a2};

static const iocshFuncDef ca_def = {"nanoFakturEBDCreateAxis", sizeof(ca_as)/sizeof(ca_as[0]), ca_as};

extern "C" void *
nanoFakturEBDCreateAxis(
	const char *controllerPortName,
	int        axisNumber,  // from 0 to (numAxes-1); as index into asynMotorAxis **pAxes_ in asynMotorController
	int        channel)     // used in commands sent to the controller hardware 
{
    void *rval = 0;

    NanoFakturEBDController *pC;
    NanoFakturEBDAxis *pAxis;
    asynMotorAxis *pAsynAxis;

	// the asyn stuff doesn't seem to be prepared for exceptions. I get segfaults
	// if constructing a controller (or axis) incurs an exception even if its
	// caught (IMHO asyn should behave as if the controller/axis never existed...)
#ifdef ASYN_CANDO_EXCEPTIONS
	try {
#endif
		pC = (NanoFakturEBDController *) findAsynPortDriver(controllerPortName);
		if (!pC) {
			printf("nanoFakturEBDCreateAxis: Error port %s not found\n", controllerPortName);
			rval = 0;
			return rval;
		}
		
		pAsynAxis = pC->getAxis(axisNumber);
		if (pAsynAxis != NULL) { // check if axis number already exists
			epicsPrintf("nanoFakturEBDCreateAxis failed: axis %u already exists\n", axisNumber);
#ifdef ASYN_CANDO_EXCEPTIONS
			THROW_(NanoFakturEBDException(EBDCommunicationError, "axis %u already exists", axisNumber));
#endif
			rval = 0;
			return rval;
		}
		pC->lock();
		pAxis = new NanoFakturEBDAxis(pC, axisNumber, channel); // create axis
		pAxis = NULL;
		pC->unlock();

#ifdef ASYN_CANDO_EXCEPTIONS
	} catch (NanoFakturEBDException &e) {
		epicsPrintf("NanoFakturEBDAxis failed (exception caught):\n%s\n", e.what());
		rval = 0;
	}
#endif

	return rval;
}

static void ca_fn(const iocshArgBuf *args)
{
	nanoFakturEBDCreateAxis(
		args[0].sval,
		args[1].ival,
		args[2].ival);
}

static void nanoFakturEBDRegister(void)
{
  iocshRegister(&cc_def, cc_fn);  // CreateController
  iocshRegister(&ca_def, ca_fn);  // CreateAxis
}

extern "C" {
epicsExportRegistrar(nanoFakturEBDRegister);
}

/* Motor driver support for nanoFAKTUR EBD-060310 Piezo Controller */

/* Derived from ACRMotorDriver.cpp by Mark Rivers, 2011/3/28 */
/* Derived from smarActMCSMotorDriver.cpp Till Straumann <strauman at slac.stanford.edu>, 9/11 */

/* Author: LiangChih Chiang <chiang.lc at nsrrc.org.tw>, 2026/01/21 */

#ifndef NANOFAKTUR_EBD_DRIVER_H
#define NANOFAKTUR_EBD_DRIVER_H

#ifdef __cplusplus

#include <stdarg.h>
#include <stdint.h>
#include <exception>

#include <asynMotorController.h>
#include <asynMotorAxis.h>

union ParamVal;
typedef union ParamVal ParamVal;


#define EXCEPTION_STR_LEN_MAX 100

enum NanoFakturEBDExceptionType {
	EBDUnknownError,
	EBDConnectionError,
	EBDCommunicationError,
};

class NanoFakturEBDException : public std::exception {
public:
	NanoFakturEBDException(NanoFakturEBDExceptionType t, const char *fmt, ...);
	NanoFakturEBDException(NanoFakturEBDExceptionType t)
		: t_(t)
		{ str_[0] = 0; }
	NanoFakturEBDException()
		: t_(EBDUnknownError)
		{ str_[0] = 0; }
	NanoFakturEBDException(NanoFakturEBDExceptionType t, const char *fmt, va_list ap);
    
	NanoFakturEBDExceptionType getType()
		const { return t_; }
	virtual const char *what()
		const throw() {return str_ ;}
        
protected:
	char str_[EXCEPTION_STR_LEN_MAX];
	NanoFakturEBDExceptionType t_;
};


class NanoFakturEBDAxis : public asynMotorAxis
{
public:
	NanoFakturEBDAxis(class NanoFakturEBDController *cnt_p, int axis, int channel);
    
	asynStatus poll(bool *moving_p);
	asynStatus move(double position, int relative, double min_vel, double max_vel, double accel);
	asynStatus moveVelocity(double min_vel, double max_vel, double accel);
	asynStatus stop(double acceleration);

	bool       getClosedLoop();
    bool       hasEncoder();

	asynStatus getUint(uint16_t cmdId, uint32_t *val_out);
	asynStatus getInt(uint16_t cmdId, int32_t *val_out);
	asynStatus getFloat(uint16_t cmdId, float *val_out);

	asynStatus setUint(uint16_t cmdId, uint32_t val);
	asynStatus setInt(uint16_t cmdId, int32_t val);
	asynStatus setFloat(uint16_t cmdId, float val);
    
    asynStatus setNone(uint16_t cmdId);
    
private:
	NanoFakturEBDController *c_p_;  // pointer to asynMotorController to which this axis belong 
	int        channel_;            // channel number used in commandsto controller; 0, 1, 2...
	asynStatus comStatus_;          // the last status of asyn communications

friend class NanoFakturEBDController;
};


class NanoFakturEBDController : public asynMotorController
{
public:
	NanoFakturEBDController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod);
    
    asynStatus cmdRsp(uint16_t cmdId, int32_t isReadCmd, size_t params_count, const uint8_t *fmts, const ParamVal *vals, size_t *params_count_out, uint8_t *fmts_out, ParamVal *vals_out);
    
    asynStatus getVal(uint16_t cmdId, uint8_t fmt0, const void *val0, void *val_out);
    asynStatus getUint(uint16_t cmdId, uint32_t *val_out);
    asynStatus getInt(uint16_t cmdId, int32_t *val_out);
    asynStatus getFloat(uint16_t cmdId, float *val_out);

    asynStatus setVal(uint16_t cmdId, uint8_t fmt0, const void *val0, uint8_t fmt1, const void *val1);
    asynStatus setUint(uint16_t cmdId, uint32_t val);
    asynStatus setInt(uint16_t cmdId, int32_t val);
    asynStatus setFloat(uint16_t cmdId, float val);

protected:
	NanoFakturEBDAxis **pAxes_;  // pointer to array of axis pointer

private:
	asynUser *asynUserMot_p_;  // for asyn communications

friend class NanoFakturEBDAxis;
};

#endif // __cplusplus
#endif // NANOFAKTUR_EBD_DRIVER_H