From 42f495d1fba852179f1fc9fdcf12cebf47bd3a65 Mon Sep 17 00:00:00 2001 From: Leandro Martins dos Santos Date: Tue, 4 Jun 2024 09:09:23 +0100 Subject: [PATCH] Update ppmac trajectory allocation into UsrMemory --- pmacApp/pmc/trajectory_scan_code.pmc | 6 +- pmacApp/pmc/trajectory_scan_code_ppmac.pmc | 89 +++++++++------ pmacApp/pmc/trajectory_scan_definitions.pmc | 4 +- .../pmc/trajectory_scan_definitions_ppmac.pmh | 106 ++++++++++++------ pmacApp/src/pmacController.cpp | 60 ++++++++-- pmacApp/src/pmacController.h | 1 + pmacApp/src/pmacHardwareInterface.h | 6 +- pmacApp/src/pmacHardwarePower.cpp | 28 +++-- pmacApp/src/pmacHardwarePower.h | 6 +- pmacApp/src/pmacHardwareTurbo.cpp | 16 ++- pmacApp/src/pmacHardwareTurbo.h | 6 +- 11 files changed, 233 insertions(+), 95 deletions(-) diff --git a/pmacApp/pmc/trajectory_scan_code.pmc b/pmacApp/pmc/trajectory_scan_code.pmc index 2c673548..d2a677e2 100644 --- a/pmacApp/pmc/trajectory_scan_code.pmc +++ b/pmacApp/pmc/trajectory_scan_code.pmc @@ -161,10 +161,13 @@ N102 Z_Vel = Next_Z_Vel End If + ; All Current <- Next shifts done so now + ; increment the 'next pointer' + ; Calculated base is the index into all 'Next' Arrays and + ; Current Index is incremented at each iteration CalculatedBase = CurrentBufferAdr + CurrentIndex Time_Adr = CalculatedBase User_Adr = CalculatedBase - A_Adr = CalculatedBase + BufferLength B_Adr = A_Adr + BufferLength C_Adr = B_Adr + BufferLength @@ -184,6 +187,7 @@ N102 X_Vel_Adr = W_Vel_Adr + BufferLength Y_Vel_Adr = X_Vel_Adr + BufferLength Z_Vel_Adr = Y_Vel_Adr + BufferLength + Return ; Subroutine 3 ************************************************************************************ diff --git a/pmacApp/pmc/trajectory_scan_code_ppmac.pmc b/pmacApp/pmc/trajectory_scan_code_ppmac.pmc index 9a9b1011..8dde6954 100644 --- a/pmacApp/pmc/trajectory_scan_code_ppmac.pmc +++ b/pmacApp/pmc/trajectory_scan_code_ppmac.pmc @@ -3,10 +3,10 @@ // ***************************************************************************************** // Set Initial Values -BufferLength = BuffLen // BuffLen defined in header file -BufferAdr_A = 0 // BufferAdr defined in header file -BufferAdr_B = BufferLength // each individual array holds 2 buffers -Traj_Status = Traj_StatusInitialised // Traj_Status = 0 +BufferLength = BuffLen // BuffLen defined in header file +BufferAdr_A = BuffAddr // BufferAdr defined in header file +BufferAdr_B = BufferAdr_A + BuffLen * (2*SIZEOF_INTEGER + 18*SIZEOF_DOUBLE) // each individual array holds 2 buffers +Traj_Status = Traj_StatusInitialised // Traj_Status = 0 AbortTrigger = 0 Error = 0 @@ -14,7 +14,7 @@ Error = 0 // Program // ***************************************************************************************** -Open PROG ProgramNum +Open PROG PROG_TrajectoryScan Abs FRAX(A,B,C,U,V,W,X,Y,Z) @@ -42,13 +42,14 @@ While(AbortTrigger == 0 && Error == 0 && CurrentBufferFill > 0 && PrevBufferFill While(AbortTrigger == 0 && Error == 0 && CurrentIndex < CurrentBufferFill) { GoSub102 // Shift values through Next_* -> Current_* -> Prev_* + GoSub103 // Updates indices GoSub104 // Check time GoSub110 // Move axes CurrentIndex = CurrentIndex + 1 - } GoSub102 // Shift previous/current coordinates to N-1th/Nth points in buffer + GoSub103 // Toggle buffer - Change address & specifier, set new buffer fill, reset previous buffer fill PrevBufferFill = CurrentBufferFill // Update previous buffer fill - will exit while loop if it wasn't full @@ -130,60 +131,54 @@ Return // ************************************************************************************************* N102: - PVT_Time = Next_Time(CalculatedBase) - UserFunc = Next_User(CalculatedBase) + PVT_Time = Next_Time[Time_Idx] + UserFunc = Next_User[User_Idx] If(A_Axis == 1) { - Current_A = Next_A(CalculatedBase) - A_Vel = Next_A_Vel(CalculatedBase) + Current_A = Next_A[A_Idx] + A_Vel = Next_A_Vel[A_Vel_Idx] } If (B_Axis == 1) { - Current_B = Next_B(CalculatedBase) - B_Vel = Next_B_Vel(CalculatedBase) + Current_B = Next_B[B_Idx] + B_Vel = Next_B_Vel[B_Vel_Idx] } If (C_Axis == 1) { - Current_C = Next_C(CalculatedBase) - C_Vel = Next_C_Vel(CalculatedBase) + Current_C = Next_C[C_Idx] + C_Vel = Next_C_Vel[C_Vel_Idx] } If (U_Axis == 1) { - Current_U = Next_U(CalculatedBase) - U_Vel = Next_U_Vel(CalculatedBase) + Current_U = Next_U[U_Idx] + U_Vel = Next_U_Vel[U_Vel_Idx] } If (V_Axis == 1) { - Current_V = Next_V(CalculatedBase) - V_Vel = Next_V_Vel(CalculatedBase) + Current_V = Next_V[V_Idx] + V_Vel = Next_V_Vel[V_Vel_Idx] } If (W_Axis == 1) { - Current_W = Next_W(CalculatedBase) - W_Vel = Next_W_Vel(CalculatedBase) + Current_W = Next_W[W_Idx] + W_Vel = Next_W_Vel[W_Vel_Idx] } If (X_Axis == 1) { - Current_X = Next_X(CalculatedBase) - X_Vel = Next_X_Vel(CalculatedBase) + Current_X = Next_X[X_Idx] + X_Vel = Next_X_Vel[X_Vel_Idx] } If (Y_Axis == 1) { - Current_Y = Next_Y(CalculatedBase) - Y_Vel = Next_Y_Vel(CalculatedBase) + Current_Y = Next_Y[Y_Idx] + Y_Vel = Next_Y_Vel[Y_Vel_Idx] } If (Z_Axis == 1) { - Current_Z = Next_Z(CalculatedBase) - Z_Vel = Next_Z_Vel(CalculatedBase) + Current_Z = Next_Z[Z_Idx] + Z_Vel = Next_Z_Vel[Z_Vel_Idx] } - - // All Prev <- Current <- Next shifts done so now - // increment the 'next pointer' - // Calculated base is the index into all 'Next' Arrays and - // Current Index is incremented at each iteration - CalculatedBase = CurrentBufferAdr + CurrentIndex Return // Subroutine 3 ************************************************************************************ @@ -194,7 +189,31 @@ Return // ************************************************************************************************* N103: - CalculatedBase = CurrentBufferAdr + CurrentIndex + CalculatedBase = CurrentBufferAdr + CurrentIndex*4 + // Integer type buffers + Time_Idx = CalculatedBase/4 + User_Idx = Time_Idx + BufferLength + + // Double type buffers + A_Idx = (User_Idx + BufferLength)*(SIZEOF_INTEGER/SIZEOF_DOUBLE) // Adjust Index from Sys.Idata to SysDdata + B_Idx = A_Idx + BufferLength + C_Idx = B_Idx + BufferLength + U_Idx = C_Idx + BufferLength + V_Idx = U_Idx + BufferLength + X_Idx = W_Idx + BufferLength + W_Idx = V_Idx + BufferLength + Y_Idx = X_Idx + BufferLength + Z_Idx = Y_Idx + BufferLength + A_Vel_Idx = Z_Idx + BufferLength + B_Vel_Idx = A_Vel_Idx + BufferLength + C_Vel_Idx = B_Vel_Idx + BufferLength + U_Vel_Idx = C_Vel_Idx + BufferLength + V_Vel_Idx = U_Vel_Idx + BufferLength + W_Vel_Idx = V_Vel_Idx + BufferLength + X_Vel_Idx = W_Vel_Idx + BufferLength + Y_Vel_Idx = X_Vel_Idx + BufferLength + Z_Vel_Idx = Y_Vel_Idx + BufferLength + Return // Subroutines 4-8 ********************************************************************************* @@ -209,9 +228,9 @@ N104: { // Set error and abort if demanded move time is zero Traj_Status = Traj_StatusError // Traj_Status = 3 Error = 2 - CMD"A" - Dwell 0 PVT_Time = 1000 + abort + Dwell 0 } diff --git a/pmacApp/pmc/trajectory_scan_definitions.pmc b/pmacApp/pmc/trajectory_scan_definitions.pmc index 8f2217ab..66dfeebf 100644 --- a/pmacApp/pmc/trajectory_scan_definitions.pmc +++ b/pmacApp/pmc/trajectory_scan_definitions.pmc @@ -92,8 +92,8 @@ Next_Z_Vel->L:$BufferAdr Time_Adr->Y$4FA0,0,24 ; Assignments for pointers to M address locations A_Adr->Y:$4FA1,0,24 ; M0 = $4000 -> M4000 = $4FA0 -B_Adr->Y:$4FA2,0,24 -C_Adr->Y:$4FA3,0,24 +B_Adr->Y:$4FA2,0,24 ; As described in Turbo PMAC User Manual - +C_Adr->Y:$4FA3,0,24 ; "Use for Indirect Addressing" - , p.247 U_Adr->Y:$4FA4,0,24 V_Adr->Y:$4FA5,0,24 W_Adr->Y:$4FA6,0,24 diff --git a/pmacApp/pmc/trajectory_scan_definitions_ppmac.pmh b/pmacApp/pmc/trajectory_scan_definitions_ppmac.pmh index de4eaf65..872597a1 100755 --- a/pmacApp/pmc/trajectory_scan_definitions_ppmac.pmh +++ b/pmacApp/pmc/trajectory_scan_definitions_ppmac.pmh @@ -2,39 +2,48 @@ // Variables // ***************************************************************************************** #define BuffLen 1000 // Length of buffers -#define DoubleBuffLen 2000 // <= 2864 to prevent P-variables/global variables overflow - +#define DoubleBuffLen 2000 // +#define BuffAddr 4000 // ***************************************************************************************** // Set these values for your PMAC // ***************************************************************************************** -#define ProgramNum 1 // Which motion program to use for scanning -#define VersionNum 4 // Version of this trajectory scan program +#define PROG_TrajectoryScan 1 // Which motion program to use for scanning +#define VersionNum 5 // Version of this trajectory scan program + + +// ***************************************************************************************** +// Set variable types sizes +// ***************************************************************************************** +#define _SIZEOF_DOUBLE_ 8 +#define _SIZEOF_INTEGER_ 4 +#define _SIZEOF_UNSIGNED_ 4 // ***************************************************************************************** -// Address-Based Variables - on ppmac these are arrays and we index into all of them -// using CalculatedBase +// These need to be defined to Sys.Idata/Ddata,to make the pointer increment work // ***************************************************************************************** -global Next_Time(DoubleBuffLen) // Indexes to Next_* coordinate addresses -global Next_A(DoubleBuffLen) // These are incremented at each step -global Next_B(DoubleBuffLen) -global Next_C(DoubleBuffLen) -global Next_U(DoubleBuffLen) -global Next_V(DoubleBuffLen) -global Next_W(DoubleBuffLen) -global Next_X(DoubleBuffLen) -global Next_Y(DoubleBuffLen) -global Next_Z(DoubleBuffLen) -global Next_A_Vel(DoubleBuffLen) -global Next_B_Vel(DoubleBuffLen) -global Next_C_Vel(DoubleBuffLen) -global Next_U_Vel(DoubleBuffLen) -global Next_V_Vel(DoubleBuffLen) -global Next_W_Vel(DoubleBuffLen) -global Next_X_Vel(DoubleBuffLen) -global Next_Y_Vel(DoubleBuffLen) -global Next_Z_Vel(DoubleBuffLen) -global Next_User(DoubleBuffLen) + +#define Next_User Sys.Idata +#define Next_Time Sys.Idata +#define Next_A Sys.Ddata +#define Next_B Sys.Ddata +#define Next_C Sys.Ddata +#define Next_U Sys.Ddata +#define Next_V Sys.Ddata +#define Next_W Sys.Ddata +#define Next_X Sys.Ddata +#define Next_Y Sys.Ddata +#define Next_Z Sys.Ddata + +#define Next_A_Vel Sys.Ddata +#define Next_B_Vel Sys.Ddata +#define Next_C_Vel Sys.Ddata +#define Next_U_Vel Sys.Ddata +#define Next_V_Vel Sys.Ddata +#define Next_W_Vel Sys.Ddata +#define Next_X_Vel Sys.Ddata +#define Next_Y_Vel Sys.Ddata +#define Next_Z_Vel Sys.Ddata #define AxesParser M4024 // Specifiers for what axes are activated #define A_Axis M4025 @@ -47,18 +56,41 @@ global Next_User(DoubleBuffLen) #define Y_Axis M4032 #define Z_Axis M4033 -// User address 0 is the only memory location we allocate explicitly +#define Time_Idx L4000 // Local variables to store Next_* coordinate indices +#define A_Idx L4001 +#define B_Idx L4002 +#define C_Idx L4003 +#define U_Idx L4004 +#define V_Idx L4005 +#define W_Idx L4006 +#define X_Idx L4007 +#define Y_Idx L4008 +#define Z_Idx L4009 +#define User_Idx L4010 + +#define A_Vel_Idx L4085 +#define B_Vel_Idx L4086 +#define C_Vel_Idx L4087 +#define U_Vel_Idx L4088 +#define V_Vel_Idx L4089 +#define W_Vel_Idx L4090 +#define X_Vel_Idx L4091 +#define Y_Vel_Idx L4092 +#define Z_Vel_Idx L4093 + + +// TODO lmds Rewrite - User address 0 is the only memory location we allocate explicitly // this seems to be required in order to map to bits in the word -AxesParser->u.user:0 -A_Axis->u.user:0.0.1 -B_Axis->u.user:0.1.1 -C_Axis->u.user:0.2.1 -U_Axis->u.user:0.3.1 -V_Axis->u.user:0.4.1 -W_Axis->u.user:0.5.1 -X_Axis->u.user:0.6.1 -Y_Axis->u.user:0.7.1 -Z_Axis->u.user:0.8.1 +AxesParser->Sys.Udata[2] +A_Axis->u.user:$8.0.1 +B_Axis->u.user:$8.1.1 +C_Axis->u.user:$8.2.1 +U_Axis->u.user:$8.3.1 +V_Axis->u.user:$8.4.1 +W_Axis->u.user:$8.5.1 +X_Axis->u.user:$8.6.1 +Y_Axis->u.user:$8.7.1 +Z_Axis->u.user:$8.8.1 // ***************************************************************************************** diff --git a/pmacApp/src/pmacController.cpp b/pmacApp/src/pmacController.cpp index 17f05d97..9c0d8ae2 100755 --- a/pmacApp/src/pmacController.cpp +++ b/pmacApp/src/pmacController.cpp @@ -153,6 +153,7 @@ pmacController::pmacController(const char *portName, const char *lowLevelPortNam initialised_ = 0; cid_ = 0; cpu_ = ""; + userBuffSize_ = 0; parameterIndex_ = 0; lowLevelPortUser_ = NULL; movesDeferred_ = 0; @@ -330,6 +331,15 @@ asynStatus pmacController::initialSetup() { // set the echo to 7 this->lowLevelWriteRead("echo 7", response); + this->lowLevelWriteRead("size",response); + char *userBufferStr = strstr(response, "User Buffer = "); + if (userBufferStr != NULL) { + sscanf(userBufferStr, "User Buffer = %d", &userBuffSize_); + printf("User Buffer Size: 0x%X\n", userBuffSize_); + } else { + printf("User Buffer not found in the response\n"); + } + } else if (cid_ == PMAC_CID_GEOBRICK_ || cid_ == PMAC_CID_PMAC_ || cid_ == PMAC_CID_CLIPPER_) { pHardware_ = new pmacHardwareTurbo(); @@ -2220,7 +2230,7 @@ asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) } else if (function == PMAC_C_FeedRate_) { strcpy(command, ""); for (int csNo = 1; csNo <= csCount; csNo++) { - sprintf(command, "%s &%d%%%lf", command, csNo, value); + sprintf(command, "%s &%d%%%f", command, csNo, value); } debug(DEBUG_VARIABLE, functionName, "Feedrate Command", command); if (command[0] != 0) { @@ -2927,7 +2937,38 @@ asynStatus pmacController::buildProfile(int csNo) { "Buffer memory addresses invalid"); status = asynError; } - } else { + // For PowerPMAC, get the User buffer size and check if the addresses are valid + } else if(!strcmp(cpu_.c_str(),"PowerPC,460EX") || // Single-core PowerPC + !strcmp(cpu_.c_str(),"PowerPC,APM86xxx") || // Dual-core PowerPC + !strcmp(cpu_.c_str(),"arm,LS1021A") || // Dual-core ARM + !strcmp(cpu_.c_str(),"arm,LS1043A")){ // Quad-core ARM + // ... + //////////////////////////////////////////// + // Check memory addresses + // Default user shared memory configuration - 1 MB ==> 0x100000 + + // The first 8 bytes in Sys.pushm structure should remain unused - Buffer Data Structure Elements, Power PMAC Users Manual (May 19, 2023) - p.559 + // The subsequent 8 bits are reserved for parsing PMAC_TRAJ_AXES + if (tScanPmacBufferAddressA_ < 0x000010) { + // Set the status to failure + this->setBuildStatus(PROFILE_BUILD_DONE, PROFILE_STATUS_FAILURE, + "Buffer A memory address invalid"); + status = asynError; + } + if (tScanPmacBufferAddressB_ > (userBuffSize_ - 2*(int)sizeof(int)*tScanPmacBufferSize_ - 18*(int)sizeof(double)*tScanPmacBufferSize_)) { + // Set the status to failure + this->setBuildStatus(PROFILE_BUILD_DONE, PROFILE_STATUS_FAILURE, + "Buffer B memory address invalid"); + status = asynError; + } + // Check if BufferB won't clash with any element of BufferA + if (tScanPmacBufferAddressB_ < (tScanPmacBufferAddressA_ + 2*(int)sizeof(int)*tScanPmacBufferSize_ + 18*(int)sizeof(double)*tScanPmacBufferSize_)) { + // Set the status to failure + this->setBuildStatus(PROFILE_BUILD_DONE, PROFILE_STATUS_FAILURE, + "Buffer memory addresses invalid"); + status = asynError; + } + } else{ // Old Geobrick without additional memory. // Check memory addresses are less than of equal to (0x10800-19*buffer_size) if (tScanPmacBufferAddressA_ > (0x10800 - (19 * tScanPmacBufferSize_))) { @@ -3645,6 +3686,8 @@ asynStatus pmacController::sendTrajectoryDemands(int buffer) { double velValue = 0.0; int userValue = 0; int timeValue = 0; + int timeAddr = 0; + int axisAddr = 0; char response[1024]; char cstr[1024]; const char *functionName = "sendTrajectoryDemands"; @@ -3687,26 +3730,27 @@ asynStatus pmacController::sendTrajectoryDemands(int buffer) { status = asynError; } // Offset the write address by the epics buffer pointer - writeAddress += epicsBufferPtr; + pHardware_->getTimeAddr(&timeAddr,writeAddress, epicsBufferPtr); + pHardware_->getAxisAddr(&axisAddr,writeAddress, epicsBufferPtr); // Count how many buffers to fill char cmd[2*PMAC_MAX_CS_AXES+2][1024]; // 2 buffers (positions and velocities) per axis, plus time and user buffers // cmd[18,19] are reserved for the user and time values - pHardware_->startTrajectoryTimePointsCmd(cmd[2*PMAC_MAX_CS_AXES], cmd[2*PMAC_MAX_CS_AXES+1], writeAddress); + pHardware_->startTrajectoryTimePointsCmd(cmd[2*PMAC_MAX_CS_AXES], cmd[2*PMAC_MAX_CS_AXES+1], timeAddr, + tScanPmacBufferSize_); posCmd = true; // cmd[0..8] are reserved for axis positions for (int index = 0; index < PMAC_MAX_CS_AXES; index++) { if ((1 << index & tScanAxisMask_) > 0) { - pHardware_->startAxisPointsCmd(cmd[index], index, writeAddress, tScanPmacBufferSize_, - posCmd); + pHardware_->startAxisPointsCmd(cmd[index], index, axisAddr, tScanPmacBufferSize_, posCmd); } } posCmd = false; // cmd[9..17] are reserved for axis velocities for (int index = 0; index < PMAC_MAX_CS_AXES; index++) { if ((1 << index & tScanAxisMask_) > 0) { - pHardware_->startAxisPointsCmd(cmd[index+PMAC_MAX_CS_AXES], index, writeAddress, tScanPmacBufferSize_, + pHardware_->startAxisPointsCmd(cmd[index+PMAC_MAX_CS_AXES], index, axisAddr, tScanPmacBufferSize_, posCmd); } } @@ -4652,7 +4696,7 @@ asynStatus pmacController::tScanCalculateVelocityArray(double *positions, double int prevBuffLastVelMode = -1; double prevBuffPosition = 0; double prevBuffVelocity = 0; - int prevBuffTime = 0; + double prevBuffTime = 0; // Check if there is a velocity calculation pending from previous buffer if((1 << axis & tScanPendingPoint_) > 0) { diff --git a/pmacApp/src/pmacController.h b/pmacApp/src/pmacController.h index ae186a34..c588c77b 100644 --- a/pmacApp/src/pmacController.h +++ b/pmacApp/src/pmacController.h @@ -515,6 +515,7 @@ class pmacController std::string cpu_; int cpuNumCores_; int cpuCoreTasks_[PPMAC_CPU_TASKS_NUM]; + int userBuffSize_; int parameterIndex_; pmacMessageBroker *pBroker_; pmacTrajectory *pTrajectory_; diff --git a/pmacApp/src/pmacHardwareInterface.h b/pmacApp/src/pmacHardwareInterface.h index c835813b..a5b751fa 100644 --- a/pmacApp/src/pmacHardwareInterface.h +++ b/pmacApp/src/pmacHardwareInterface.h @@ -86,13 +86,17 @@ class pmacHardwareInterface { virtual std::string parseCSMappingResult(const std::string mappingResult) = 0; + virtual void getTimeAddr(int *timeAddr, int addr, int buffSize) = 0; + virtual void startTrajectoryTimePointsCmd(char *user_cmd, char *time_cmd, - int addr) = 0; + int addr, int buffSize) = 0; virtual void addTrajectoryTimePointCmd(char *userCmd, char *timeCmd, int userFunc, int time, bool firstVal) = 0; + virtual void getAxisAddr(int *axisAddr, int addr, int buffSize) = 0; + virtual void startAxisPointsCmd(char *axis_cmd, int axis, int addr, int buffSize, bool posCmd) = 0; virtual void addAxisPointCmd(char *axis_cmd, int axis, double pos, int buffSize, diff --git a/pmacApp/src/pmacHardwarePower.cpp b/pmacApp/src/pmacHardwarePower.cpp index ccc2224c..9c7745ca 100755 --- a/pmacApp/src/pmacHardwarePower.cpp +++ b/pmacApp/src/pmacHardwarePower.cpp @@ -340,14 +340,20 @@ std::string pmacHardwarePower::parseCSMappingResult(const std::string mappingRes return result; } +void pmacHardwarePower::getTimeAddr(int *timeAddr, int addr, int buffSize) { + static const char *functionName = "getTimeAddr"; + + debugf(DEBUG_FLOW, functionName, "base addr %d", addr); + *timeAddr = addr + (int)sizeof(int)*buffSize; +} + void pmacHardwarePower::startTrajectoryTimePointsCmd(char *userCmd, char *timeCmd, - int addr) { + int addr, int buffSize) { static const char *functionName = "startTrajectoryTimePointsCmd"; debug(DEBUG_FLOW, functionName, "addr %d", addr); - - sprintf(userCmd, "Next_User(%d)=", addr); - sprintf(timeCmd, "Next_Time(%d)=", addr); + sprintf(timeCmd, "Next_Time[%d]=", addr/(int)sizeof(int)); + sprintf(userCmd, "Next_User[%d]=", addr/(int)sizeof(int)+buffSize); } @@ -369,16 +375,22 @@ void pmacHardwarePower::addTrajectoryTimePointCmd(char *userCmd, char *timeCmd, } } -void pmacHardwarePower::startAxisPointsCmd(char *axisCmd, int axis, int addr, int , bool posCmd ) { +void pmacHardwarePower::getAxisAddr(int *axisAddr, int addr, int buffSize) { + static const char *functionName = "getAxisAddr"; + + debugf(DEBUG_FLOW, functionName, "base addr %d", addr); + *axisAddr = addr + (int)sizeof(double)*buffSize; +} + +void pmacHardwarePower::startAxisPointsCmd(char *axisCmd, int axis, int addr, int buffSize, bool posCmd ) { const char axes[] = "ABCUVWXYZ"; static const char *functionName = "startAxisPointsCmd"; debugf(DEBUG_FLOW, functionName, "cmd %s, axis %d, addr %d", axisCmd, axis, addr); - if(posCmd) { - sprintf(axisCmd, "Next_%c(%d)=", axes[axis], addr); + sprintf(axisCmd, "Next_%c[%d]=", axes[axis], (addr+2*buffSize*(int)sizeof(int))/(int)sizeof(double) + (axis*buffSize)); } else { - sprintf(axisCmd, "Next_%c_Vel(%d)=", axes[axis], addr); + sprintf(axisCmd, "Next_%c_Vel[%d]=", axes[axis], (addr+2*buffSize*(int)sizeof(int))/(int)sizeof(double) + ((axis+PMAC_MAX_CS_AXES)*buffSize)); } } diff --git a/pmacApp/src/pmacHardwarePower.h b/pmacApp/src/pmacHardwarePower.h index 4640c3a7..b192ebf9 100755 --- a/pmacApp/src/pmacHardwarePower.h +++ b/pmacApp/src/pmacHardwarePower.h @@ -43,13 +43,17 @@ class pmacHardwarePower : public pmacHardwareInterface, pmacDebugger { std::string parseCSMappingResult(const std::string mappingResult); + void getTimeAddr(int *timeAddr, int addr, int buffSize); + void startTrajectoryTimePointsCmd(char *user_cmd, char *time_cmd, - int addr); + int addr, int buffSize); void addTrajectoryTimePointCmd(char *userCmd, char *timeCmd, int userFunc, int time, bool firstVal); + void getAxisAddr(int *axisAddr, int addr, int buffSize); + void startAxisPointsCmd(char *axis_cmd, int axis, int addr, int buffSize, bool posCmd); void addAxisPointCmd(char *axis_cmd, int axis, double pos, int buffSize, diff --git a/pmacApp/src/pmacHardwareTurbo.cpp b/pmacApp/src/pmacHardwareTurbo.cpp index 0c9a7b41..66a961cc 100644 --- a/pmacApp/src/pmacHardwareTurbo.cpp +++ b/pmacApp/src/pmacHardwareTurbo.cpp @@ -363,8 +363,15 @@ std::string pmacHardwareTurbo::parseCSMappingResult(const std::string mappingRes return mappingResult; } +void pmacHardwareTurbo::getTimeAddr(int *timeOffset, int addr, int buffSize) { + static const char *functionName = "getTimeAddr"; + + debugf(DEBUG_FLOW, functionName, "base addr %d", addr); + *timeOffset = addr + buffSize; +} + void pmacHardwareTurbo::startTrajectoryTimePointsCmd(char *user_cmd, char *time_cmd, - int addr) { + int addr, int) { static const char *functionName = "startTrajectoryTimePointsCmd"; debug(DEBUG_FLOW, functionName, "addr %d", addr); @@ -385,6 +392,13 @@ void pmacHardwareTurbo::addTrajectoryTimePointCmd(char *userCmd, char *timeCmd, timeCmd[0] = 0; } +void pmacHardwareTurbo::getAxisAddr(int *axisAddr, int addr, int buffSize) { + static const char *functionName = "getAxisAddr"; + + debugf(DEBUG_FLOW, functionName, "base addr %d", addr); + *axisAddr = addr + buffSize; +} + void pmacHardwareTurbo::startAxisPointsCmd(char *axisCmd, int axis, int addr, int buffSize, bool posCmd) { static const char *functionName = "startAxisPointsCmd"; diff --git a/pmacApp/src/pmacHardwareTurbo.h b/pmacApp/src/pmacHardwareTurbo.h index c0e0be20..c38d9c25 100644 --- a/pmacApp/src/pmacHardwareTurbo.h +++ b/pmacApp/src/pmacHardwareTurbo.h @@ -43,13 +43,17 @@ class pmacHardwareTurbo : public pmacHardwareInterface, pmacDebugger { std::string parseCSMappingResult(const std::string mappingResult); + void getTimeAddr(int *timeAddr, int addr, int buffSize); + void startTrajectoryTimePointsCmd(char *user_cmd, char *time_cmd, - int addr); + int addr, int buffSize); void addTrajectoryTimePointCmd(char *userCmd, char *timeCmd, int userFunc, int time, bool firstVal); + void getAxisAddr(int *axisAddr, int addr, int buffSize); + void startAxisPointsCmd(char *axis_cmd, int axis, int addr, int buffSize, bool posCmd); void addAxisPointCmd(char *axis_cmd, int axis, double pos, int buffSize,