Adjusted driver with auxiliary axes offsets

src/detectorTowerController.cpp

@@ -47,45 +47,73 @@ detectorTowerController::detectorTowerController(
                   stringifyAsynStatus(status));
         exit(-1);
     }
+
+    status = createParam("MOTOR_OFFSET", asynParamFloat64, &motorTargetOffset_);
+    if (status != asynSuccess) {
+        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "Controller \"%s\" => %s, line %d\nFATAL ERROR (creating a "
+                  "parameter failed with %s).\nTerminating IOC",
+                  portName, __PRETTY_FUNCTION__, __LINE__,
+                  stringifyAsynStatus(status));
+        exit(-1);
+    }
 }
 
 asynStatus detectorTowerController::readInt32(asynUser *pasynUser,
                                               epicsInt32 *value) {
 
-    // Check if the axis is a detectorTowerAxis
-    detectorTowerAxis *axis = getDetectorTowerAxis(pasynUser);
-    if (axis == nullptr) {
-        // This is apparently a "normal" turboPmacAxis or an auxiliaryAxis
-        return turboPmacController::readInt32(pasynUser, value);
-    } else {
-        // The detector tower cannot be disabled
-        if (pasynUser->reason == motorCanDisable_) {
+    // The detector tower or its auxiliary axes cannot be disabled
+    if (pasynUser->reason == motorCanDisable_) {
+        detectorTowerAxis *dTAxis = getDetectorTowerAxis(pasynUser);
+        if (dTAxis != nullptr) {
+            *value = 0;
+            return asynSuccess;
+        }
+        detectorTowerAuxiliaryAxis *dTAAxis =
+            getDetectorTowerAuxiliaryAxis(pasynUser);
+        if (dTAAxis != nullptr) {
             *value = 0;
             return asynSuccess;
-        } else {
-            return turboPmacController::readInt32(pasynUser, value);
         }
     }
+    return turboPmacController::readInt32(pasynUser, value);
 }
 
 asynStatus detectorTowerController::writeInt32(asynUser *pasynUser,
                                                epicsInt32 value) {
+
+    // =====================================================================
+
+    if (pasynUser->reason == motorCanDisable_) {
+        detectorTowerAxis *dTAxis = getDetectorTowerAxis(pasynUser);
+        if (dTAxis != nullptr) {
+            return dTAxis->toggleWorkingChangerState(value);
+        }
+    }
+    return turboPmacController::writeInt32(pasynUser, value);
+}
+
+asynStatus detectorTowerController::writeFloat64(asynUser *pasynUser,
+                                                 epicsFloat64 value) {
     int function = pasynUser->reason;
 
     // =====================================================================
 
-    detectorTowerAxis *axis = getDetectorTowerAxis(pasynUser);
+    if (function == motorTargetOffset_) {
 
-    if (axis == nullptr) {
-        // This is apparently a "normal" turboPmacAxis or an auxiliaryAxis
-        return turboPmacController::writeInt32(pasynUser, value);
-    } else {
-        if (function == changeState_) {
-            return axis->toggleWorkingChangerState(value);
-        } else {
-            return turboPmacController::writeInt32(pasynUser, value);
+        // Write into the parameter library and trigger a movement cycle, if the
+        // writing succeeded
+        asynStatus status = turboPmacController::writeFloat64(pasynUser, value);
+        if (status == asynSuccess) {
+            detectorTowerAxis *dTAxis = getDetectorTowerAxis(pasynUser);
+            if (dTAxis != nullptr) {
+                dTAxis->receivedTarget_ = true;
+            }
         }
+        return status;
     }
+
+    return turboPmacController::writeFloat64(pasynUser, value);
 }
 
 /*
@@ -108,6 +136,27 @@ detectorTowerAxis *detectorTowerController::getDetectorTowerAxis(int axisNo) {
     return dynamic_cast<detectorTowerAxis *>(asynAxis);
 }
 
+/*
+Access one of the axes of the controller via the axis adress stored in asynUser.
+If the axis does not exist or is not a Axis, a nullptr is returned and an
+error is emitted.
+*/
+detectorTowerAuxiliaryAxis *
+detectorTowerController::getDetectorTowerAuxiliaryAxis(asynUser *pasynUser) {
+    asynMotorAxis *asynAxis = asynMotorController::getAxis(pasynUser);
+    return dynamic_cast<detectorTowerAuxiliaryAxis *>(asynAxis);
+}
+
+/*
+Access one of the axes of the controller via the axis index.
+If the axis does not exist or is not a Axis, the function must return Null
+*/
+detectorTowerAuxiliaryAxis *
+detectorTowerController::getDetectorTowerAuxiliaryAxis(int axisNo) {
+    asynMotorAxis *asynAxis = asynMotorController::getAxis(axisNo);
+    return dynamic_cast<detectorTowerAuxiliaryAxis *>(asynAxis);
+}
+
 /*************************************************************************************/
 /** The following functions are C-wrappers, and can be called directly from
  * iocsh */