Use frame mode to get resolution for depth buffer.

cbits/freenect-helpers.c

@@ -36,3 +36,11 @@ int set_freenect_depth_mode(freenect_device* dev,freenect_frame_mode* mode)
 {
   return freenect_set_depth_mode(dev,*mode);
 };
+
+/*
+ * Get the depth resolution of a device.
+ */
+uint32_t get_freenect_depth_resolution(freenect_device* dev){
+  freenect_frame_mode mode = freenect_get_current_depth_mode(dev);
+  return mode.resolution;
+}

src/Freenect.hs

@@ -31,7 +31,7 @@ module Freenect
 
 import Freenect.FFI
 
-import Control.Exception
+import Control.Exception (bracket,throw,Exception(..))
 import Control.Monad
 import Data.Bits
 import Data.IORef
@@ -72,6 +72,7 @@ data FreenectException
   | StartDepthProblem        -- ^ Problem starting the depth stream.
   | UnableToSetTilt          -- ^ Unable to set the tilt.
   | SetDepthMode             -- ^ Unable to set the depth mode.
+  | DepthModeNotSet          -- ^ You didn't set the depth mode.
     deriving (Show,Typeable)
 instance Exception FreenectException
 
@@ -207,12 +208,20 @@ setLogLevel level = withC $ \ptr -> do
 setDepthCallback :: Device -> (Vector Word16 -> Word32 -> IO ()) -> IO ()
 setDepthCallback d callback = flip withD d $ \dptr -> do
   dptr <- peek dptr
+  resolution <- get_freenect_depth_resolution dptr
+  let !size = resolutionToSize (toEnum (fromIntegral resolution))
   callbackPtr <- wrapDepthCallback $ \_ payloadptr timestamp -> do
     fptr <- newForeignPtr_ payloadptr
-    let !vector = unsafeFromForeignPtr fptr 0 (640*480)
+    let !vector = unsafeFromForeignPtr fptr 0 size
     callback vector timestamp
   freenect_set_depth_callback dptr callbackPtr
 
+-- | Resolution to size.
+resolutionToSize :: Resolution -> Int
+resolutionToSize Low    = 320  * 240
+resolutionToSize Medium = 640  * 480
+resolutionToSize High   = 1280 * 1024
+
 -- | Start the depth information stream for a device.
 startDepth :: Device -> IO ()
 startDepth = withD $ \ptr -> succeed StartDepthProblem (return ()) $ do

src/Freenect/FFI.hs

@@ -90,3 +90,7 @@ foreign import ccall
 foreign import ccall
   "freenect.h set_freenect_depth_mode"
   set_freenect_depth_mode :: Ptr DeviceStruct -> Ptr FrameMode -> IO CInt
+
+foreign import ccall
+  "freenect.h get_freenect_depth_resolution"
+  get_freenect_depth_resolution :: Ptr DeviceStruct -> IO CInt

src/Main.hs

@@ -22,11 +22,11 @@ main =
     withDevice context index $ \device -> do
       printf "Opened device %d.\n" index
       done <- newIORef False
+      setDepthMode device Medium ElevenBit
       setDepthCallback device $ \payload timestamp -> do
         printf "Payload: %s\n" (take 100 $ show payload)
         writeIORef done True
       printf "Setted depth callback.\n"
-      setDepthMode device Medium ElevenBit
       startDepth device
       printf "Started depth stream.\n"
       printf "Processing…\n"