Index: trunk/psModules/src/camera/pmFPAMaskWeight.c
===================================================================
--- trunk/psModules/src/camera/pmFPAMaskWeight.c	(revision 17732)
+++ trunk/psModules/src/camera/pmFPAMaskWeight.c	(revision 19163)
@@ -12,4 +12,6 @@
 #include "pmHDUGenerate.h"
 #include "pmFPAMaskWeight.h"
+
+#define PIXELS_BUFFER 100               // Size of buffer for allocating pixel lists
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -431,2 +433,81 @@
     return true;
 }
+
+
+bool pmReadoutInterpolateBadPixels(pmReadout *readout, psMaskType maskVal, psImageInterpolateMode mode,
+                                   float poorFrac, psMaskType maskPoor, psMaskType maskBad)
+{
+    PM_ASSERT_READOUT_NON_NULL(readout, false);
+    PM_ASSERT_READOUT_IMAGE(readout, false);
+    PM_ASSERT_READOUT_MASK(readout, false);
+    if (!maskVal) {
+        return true;
+    }
+
+    psImage *image = readout->image;    // Image
+    psImage *mask = readout->mask;      // Mask
+    psImage *weight = readout->weight;  // Weight map
+
+    psImageInterpolateOptions *interp = psImageInterpolateOptionsAlloc(mode, image, weight, mask, maskVal,
+                                                                       NAN, NAN, maskBad, maskPoor, poorFrac);
+    interp->shifting = false;           // Turn off "exact shifts" so we get proper interpolation
+
+    int numCols = mask->numCols, numRows = mask->numRows; // Size of image
+
+    psPixels *pixels = psPixelsAllocEmpty(PIXELS_BUFFER); // Pixels that have been interpolated
+    psVector *imagePix = psVectorAllocEmpty(PIXELS_BUFFER, PS_TYPE_F32); // Corresponding values for image
+    psVector *weightPix = psVectorAllocEmpty(PIXELS_BUFFER, PS_TYPE_F32); // Corresponding values for weight
+    psVector *maskPix = psVectorAllocEmpty(PIXELS_BUFFER, PS_TYPE_MASK); // Corresponding values for mask
+
+    long numBad = 0;                    // Number of bad pixels interpolated
+    for (int y = 0; y < numRows; y++) {
+        for (int x = 0; x < numCols; x++) {
+            if (mask->data.PS_TYPE_MASK_DATA[y][x] & maskVal) {
+                double imageValue, weightValue; // Image and weight value from interpolation
+                psMaskType maskValue = 0; // Mask value from interpolation
+                psImageInterpolateStatus status = psImageInterpolate(&imageValue, &weightValue, &maskValue,
+                                                                     x, y, interp);
+                if (status == PS_INTERPOLATE_STATUS_ERROR || status == PS_INTERPOLATE_STATUS_OFF) {
+                    psError(PS_ERR_UNKNOWN, false, "Unable to interpolate readout at %d,%d", x, y);
+                    psFree(interp);
+                    psFree(pixels);
+                    psFree(imagePix);
+                    psFree(weightPix);
+                    psFree(maskPix);
+                    return false;
+                }
+                if (status == PS_INTERPOLATE_STATUS_BAD) {
+                    // It's still bad: couldn't interpolate enough
+                    continue;
+                }
+
+                pixels = psPixelsAdd(pixels, PIXELS_BUFFER, x, y);
+                imagePix = psVectorExtend(imagePix, PIXELS_BUFFER, 1);
+                weightPix = psVectorExtend(weightPix, PIXELS_BUFFER, 1);
+                maskPix = psVectorExtend(maskPix, PIXELS_BUFFER, 1);
+                imagePix->data.F32[numBad] = imageValue;
+                weightPix->data.F32[numBad] = weightValue;
+                maskPix->data.PS_TYPE_MASK_DATA[numBad] = maskValue;
+                numBad++;
+            }
+        }
+    }
+
+    psFree(interp);
+
+    for (long i = 0; i < numBad; i++) {
+        int x = pixels->data[i].x, y = pixels->data[i].y; // Coordinates of pixel
+        image->data.F32[y][x] = imagePix->data.F32[i];
+        weight->data.F32[y][x] = weightPix->data.F32[i];
+        mask->data.PS_TYPE_MASK_DATA[y][x] = maskPix->data.PS_TYPE_MASK_DATA[i];
+    }
+
+    psFree(pixels);
+    psFree(imagePix);
+    psFree(weightPix);
+    psFree(maskPix);
+
+    psLogMsg("psModules.camera", PS_LOG_INFO, "Interpolated over %ld pixels", numBad);
+
+    return true;
+}