From 668e3e408ae30e32ef7566b43a334d92ac127b60 Mon Sep 17 00:00:00 2001 From: bhack Date: Wed, 12 Jun 2013 13:45:05 +0200 Subject: [PATCH] Add Autoexposure AutoWhiteBalance ColorCorrection Add new register control to C and C++ api --- include/libfreenect.h | 9 +++- src/cameras.c | 96 +++++++++++++++++++++++++++++++++++- wrappers/cpp/libfreenect.hpp | 21 ++++++++ 3 files changed, 123 insertions(+), 3 deletions(-) diff --git a/include/libfreenect.h b/include/libfreenect.h index 4dfb6b19..7319bef8 100644 --- a/include/libfreenect.h +++ b/include/libfreenect.h @@ -614,7 +614,14 @@ FREENECTAPI freenect_frame_mode freenect_find_depth_mode(freenect_resolution res * @return 0 on success, < 0 if error */ FREENECTAPI int freenect_set_depth_mode(freenect_device* dev, const freenect_frame_mode mode); - +//added by zhy +FREENECTAPI int freenect_set_auto_exposure (freenect_device * dev, int enabled); +FREENECTAPI int freenect_set_color_correction (freenect_device * dev, int enabled); +FREENECTAPI int freenect_set_auto_white_balance (freenect_device * dev, int enabled); +FREENECTAPI int freenect_get_auto_exposure (freenect_device * dev); +FREENECTAPI int freenect_get_color_correction (freenect_device * dev); +FREENECTAPI int freenect_get_auto_white_balance (freenect_device * dev); +//add end #ifdef __cplusplus } #endif diff --git a/src/cameras.c b/src/cameras.c index 7886a332..0076a617 100644 --- a/src/cameras.c +++ b/src/cameras.c @@ -688,8 +688,7 @@ static int send_cmd(freenect_device *dev, uint16_t cmd, void *cmdbuf, unsigned i do { actual_len = fnusb_control(&dev->usb_cam, 0xc0, 0, 0, 0, ibuf, 0x200); - FN_FLOOD("actual_len: %d\n", actual_len); - } while ((actual_len == 0) || (actual_len == 0x200)); + } while (actual_len == 0); FN_SPEW("Control reply: %d\n", res); if (actual_len < (int)sizeof(*rhdr)) { FN_ERROR("send_cmd: Input control transfer failed (%d)\n", res); @@ -1350,6 +1349,9 @@ FN_INTERNAL int freenect_camera_init(freenect_device *dev) } res = freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)); res = freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT)); + //added by zhy + res = freenect_set_auto_exposure(dev,0); + //add end res = freenect_fetch_reg_const_shift(dev); if (res < 0) { FN_ERROR("freenect_camera_init(): Failed to fetch const shift for device\n"); @@ -1379,3 +1381,93 @@ FN_INTERNAL int freenect_camera_teardown(freenect_device *dev) freenect_destroy_registration(&(dev->registration)); return 0; } + + +static uint16_t write_cmos_register(freenect_device *dev, uint16_t reg, uint16_t value) +{ + freenect_context *ctx = dev->parent; + uint16_t replybuf[0x200]; + uint16_t cmdbuf[3]; + cmdbuf[0] = 1; + cmdbuf[1] = reg | 0x8000; + cmdbuf[2] = value; + int res = send_cmd(dev, 0x95, cmdbuf, 6, replybuf, 6); + if (res < 0) + { + FN_ERROR("read_cmos_register: send_cmd returned %d\n", res); + return -1; + } + return 0; + } + + static uint16_t read_cmos_register(freenect_device *dev, uint16_t reg) + { + freenect_context *ctx = dev->parent; + uint16_t replybuf[0x200]; + uint16_t cmdbuf[3]; + cmdbuf[0] = 1; + cmdbuf[1] = reg & 0x7fff; + cmdbuf[2] = 0; + int res = send_cmd(dev, 0x95, cmdbuf, 6, replybuf, 6); + if (res < 0) + { + FN_ERROR("read_cmos_register: send_cmd returned %d\n", res); + } + return replybuf[2]; +} + + +int +freenect_set_auto_exposure (freenect_device * dev, int enabled) +{ + uint16_t r = read_cmos_register(dev, 0x0106); + if (enabled) + r |= 1 << 14; // set bit 14 to enable auto exposure + else + r &= ~(1 << 14); // clear bit 14 to disable auto exposure + write_cmos_register(dev, 0x0106, r); + return (0); + } + + int + freenect_set_color_correction (freenect_device * dev, int enabled) + { + uint16_t r = read_cmos_register(dev, 0x0106); + if (enabled) + r &= ~(1 << 4); // clear bit 4 for normal color processing + else + r |= 1 << 4; // set bit 4 to output "raw" color bypassing color correction + write_cmos_register(dev, 0x0106, r); + return (0); + } + + int + freenect_set_auto_white_balance (freenect_device * dev, int enabled) + { + uint16_t r = read_cmos_register(dev, 0x0106); + if (enabled) + r |= 1 << 1; // set bit 1 to enable auto white balance + else + r &= ~(1 << 1); // clear bit 1 to disable auto white balance + write_cmos_register(dev, 0x0106, r); + return (0); + } + + int + freenect_get_auto_exposure (freenect_device * dev) + { + return read_cmos_register(dev, 0x0106) & (1 << 14); + } + + int + freenect_get_color_correction (freenect_device * dev) + { + return read_cmos_register(dev, 0x0106) & (1 << 4); + } + + int + freenect_get_auto_white_balance (freenect_device * dev) + { + return read_cmos_register(dev, 0x0106) & (1 << 1); +} + diff --git a/wrappers/cpp/libfreenect.hpp b/wrappers/cpp/libfreenect.hpp index 0a5f637e..5964c2ab 100644 --- a/wrappers/cpp/libfreenect.hpp +++ b/wrappers/cpp/libfreenect.hpp @@ -95,6 +95,27 @@ namespace Freenect { void updateState() { if (freenect_update_tilt_state(m_dev) < 0) throw std::runtime_error("Cannot update device state"); } + //add new register settings + void setAutoExposure (int enabled) { + if(freenect_set_auto_exposure (m_dev,enabled)<0) throw std::runtime_error("Cannot set auto exposure"); + } + void getAutoExposure () { + if(freenect_get_auto_exposure (m_dev)<0) throw std::runtime_error("Cannot get auto exposure"); + } + void setAutoWhiteBalance (int enabled) { + if(freenect_set_auto_white_balance (m_dev,enabled)<0) throw std::runtime_error("Cannot set white balance"); + } + void getAutoWhiteBalance () { + if(freenect_get_auto_white_balance (m_dev)<0) throw std::runtime_error("Cannot get white balance"); + } + void setColorCorrection (int enabled) { + if(freenect_set_color_correction (m_dev,enabled)<0) throw std::runtime_error("Cannot set color correction"); + } + void getColorCorrection () { + if(freenect_get_color_correction (m_dev)<0) throw std::runtime_error("Cannot get color correction"); + } + //add end + FreenectTiltState getState() const { return FreenectTiltState(freenect_get_tilt_state(m_dev)); }