00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <cams/nao.h>
00026 #include <fvutils/system/camargp.h>
00027 #include <utils/logging/liblogger.h>
00028 #include <core/exceptions/software.h>
00029
00030 #include <fcntl.h>
00031 #include <sys/ioctl.h>
00032 #include <linux/i2c-dev.h>
00033 #include <linux/types.h>
00034
00035 #include <cstring>
00036 #include <cstdlib>
00037 #include <vector>
00038
00039 using namespace fawkes;
00040
00041 #define V4L2_CID_AUTOEXPOSURE (V4L2_CID_BASE+32)
00042 #define V4L2_CID_CAM_INIT (V4L2_CID_BASE+33)
00043 #define V4L2_CID_EXPOSURE_CORRECTION (V4L2_CID_BASE+34)
00044 #define V4L2_CID_AEC_ALGORITHM (V4L2_CID_BASE+35)
00045
00046 #ifndef I2C_FUNC_SMBUS_READ_BYTE_DATA
00047 #include <linux/i2c.h>
00048
00049
00050
00051
00052
00053 static inline __s32
00054 i2c_smbus_access(int file, char read_write, __u8 command,
00055 int size, union i2c_smbus_data *data)
00056 {
00057 struct i2c_smbus_ioctl_data args;
00058
00059 args.read_write = read_write;
00060 args.command = command;
00061 args.size = size;
00062 args.data = data;
00063 return ioctl(file,I2C_SMBUS,&args);
00064 }
00065
00066 static inline __s32
00067 i2c_smbus_read_byte_data(int file, __u8 command)
00068 {
00069 union i2c_smbus_data data;
00070 if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
00071 I2C_SMBUS_BYTE_DATA,&data))
00072 return -1;
00073 else
00074 return 0x0FF & data.byte;
00075 }
00076
00077 static inline __s32
00078 i2c_smbus_write_block_data(int file, __u8 command,
00079 __u8 length, __u8 *values)
00080 {
00081 union i2c_smbus_data data;
00082 int i;
00083 if (length > 32)
00084 length = 32;
00085 for (i = 1; i <= length; i++)
00086 data.block[i] = values[i-1];
00087 data.block[0] = length;
00088 return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
00089 I2C_SMBUS_BLOCK_DATA, &data);
00090 }
00091
00092
00093 #endif
00094
00095 namespace firevision {
00096 #if 0
00097 }
00098 #endif
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 NaoCamera::NaoCamera(const CameraArgumentParser *cap)
00118 : V4L2Camera(cap)
00119 {
00120 if (cap->has("i2c_device")) __i2c_device_name = strdup(cap->get("i2c_device").c_str());
00121 else throw MissingParameterException("NaoCamera: Missing I2C device");
00122
00123 __can_switch_cam = false;
00124 __cam_id = 2;
00125
00126 if (cap->has("cam"))
00127 {
00128 if (strcasecmp(cap->get("cam").c_str(), "brow") == 0) __cam_id = 1;
00129 }
00130
00131 int dev = open_dev(__i2c_device_name);
00132
00133
00134 int val = i2c_smbus_read_byte_data(dev, 170);
00135 if (val == -1) close_dev(dev, "NaoCamera: Error reading dsPic version from I2C");
00136 if (val < 2)
00137 {
00138 LibLogger::log_info("NaoCamera", "Nao V2 found - No camera switching possible");
00139 close_dev(dev);
00140 return;
00141 }
00142 __can_switch_cam = true;
00143 LibLogger::log_debug("NaoCamera", "Nao V3 found - Trying to switch to camera %d", __cam_id);
00144
00145 val = get_open_cam_id(dev);
00146
00147 if (val == __cam_id)
00148 {
00149 LibLogger::log_debug("NaoCamera", "Correct camera already chosen");
00150 }
00151 else
00152 {
00153
00154 switch_to_cam_id(dev, __cam_id);
00155 }
00156 close_dev(dev);
00157
00158
00159
00160 init_cam(_device_name);
00161 }
00162
00163 NaoCamera::~NaoCamera()
00164 {
00165 free(__i2c_device_name);
00166 }
00167
00168
00169
00170
00171
00172
00173 int NaoCamera::open_dev(const char *i2c)
00174 {
00175
00176 int dev = ::open(i2c, O_RDWR);
00177 if (dev < 0) throw Exception("NaoCamera: Error opening I2C for connection to dsPIC");
00178 if (ioctl(dev, I2C_SLAVE, DSPIC_I2C_ADDR) < 0) close_dev(dev, "NaoCamera: Can't connect I2C to dsPIC");
00179 return dev;
00180 }
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190 void NaoCamera::close_dev(int dev, const char *error)
00191 {
00192 if (::close(dev) < 0) throw fawkes::Exception("NaoCamera: Error closing device");
00193 if (error) throw fawkes::Exception(error);
00194 }
00195
00196
00197
00198
00199
00200 int NaoCamera::get_open_cam_id(int dev)
00201 {
00202
00203 int cid = i2c_smbus_read_byte_data(dev, 220);
00204 if (cid == -1) close_dev(dev, "Error reading active cam from I2C");
00205 return cid;
00206 }
00207
00208
00209
00210
00211
00212
00213 void NaoCamera::switch_to_cam_id(int dev, int cam_id)
00214 {
00215 unsigned char cmd[2];
00216 cmd[0] = cam_id;
00217 cmd[1] = 0;
00218 int size = i2c_smbus_write_block_data(dev, 220, 1, cmd);
00219 if (size == -1) close_dev(dev, "NaoCamera: Error switching to other camera");
00220 }
00221
00222
00223
00224
00225
00226 void NaoCamera::init_cam(const char *cam)
00227 {
00228 int dev = ::open(cam, O_RDWR);
00229 if (dev < 0) throw Exception("NaoCamera: Error opening Camera");
00230
00231 struct v4l2_control control;
00232 memset(&control, 0, sizeof(control));
00233
00234 control.id = V4L2_CID_CAM_INIT;
00235 control.value = 0;
00236
00237 if (ioctl(dev, VIDIOC_S_CTRL, &control)) close_dev(dev, "Error setting other camera to default parameters");
00238
00239 close_dev(dev);
00240 }
00241
00242
00243
00244
00245
00246
00247
00248 unsigned char NaoCamera::source()
00249 {
00250 int dev = open_dev(__i2c_device_name);
00251 __cam_id = get_open_cam_id(dev);
00252 close_dev(dev);
00253
00254 return static_cast<unsigned char>(__cam_id);
00255 }
00256
00257
00258
00259
00260
00261
00262
00263
00264 void NaoCamera::set_source(unsigned char source)
00265 {
00266 if (source == __cam_id)
00267 {
00268 LibLogger::log_debug("NaoCamera", "Correct camera already chosen");
00269 return;
00270 }
00271
00272 int dev = open_dev(__i2c_device_name);
00273 switch_to_cam_id(dev, source);
00274 close_dev(dev);
00275 init_cam(_device_name);
00276 }
00277
00278
00279
00280
00281
00282 bool NaoCamera::auto_exposure()
00283 {
00284 return get_one_control("AEC", V4L2_CID_AUTOEXPOSURE);
00285 }
00286
00287
00288
00289
00290
00291 void NaoCamera::set_auto_exposure(bool enabled)
00292 {
00293 LibLogger::log_debug("NaoCamera", (enabled ? "enabling AEC" : "disabling AEC"));
00294
00295 set_one_control("AEC", V4L2_CID_AUTOEXPOSURE, (enabled ? 1 : 0));
00296 }
00297
00298 }
00299