Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: Add support for mt9v117 camera

Browse Source 
The camera sensor is connected on i2c bus for config
and on a parallel bus on the main SoC.
Currently, the i2c driver remains userland, but this is intended to
change in the future. The v4l2_subdev part is the way to go in the future
and it is the mainline way of configuring i2c camera sensors on Linux.
Currently only the max framerate is supported because it is the one that
is to be used on the bebop optical flow.
This commit is contained in:
Julien BERAUD 2015-11-23 20:22:39 +01:00 committed by Andrew Tridgell
parent 736f78a7a2
commit c84d9bf702
5 changed files with 718 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_HAL_Linux/CameraSensor.cpp Normal file
View File

@ -0,0 +1,54 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "CameraSensor.h"
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/v4l2-subdev.h>
using namespace Linux;
bool CameraSensor::set_format(uint32_t width, uint32_t height, uint32_t format)
{
struct v4l2_subdev_format fmt;
int ret, fd;
fd = open(_device_path, O_RDWR);
if (fd < 0) {
return false;
}
memset(&fmt, 0, sizeof(fmt));
fmt.pad = 0;
fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
fmt.format.width = width;
fmt.format.height = height;
fmt.format.code = format;
ret = ioctl(fd, VIDIOC_SUBDEV_S_FMT, &fmt);
if (ret < 0) {
return false;
}
return true;
}
#endif

29
libraries/AP_HAL_Linux/CameraSensor.h Normal file
View File

@ -0,0 +1,29 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __CAMERASENSOR_H__
#define __CAMERASENSOR_H__
#include "AP_HAL_Linux.h"
class Linux::CameraSensor {
public:
CameraSensor(const char *device_path) {_device_path = device_path;}
bool set_format(uint32_t width, uint32_t height, uint32_t format);
bool set_crop(uint32_t left, uint32_t top);
private:
const char *_device_path;
};
#endif

418
libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp Normal file
View File

@ -0,0 +1,418 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "GPIO.h"
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#include "CameraSensor_Mt9v117.h"
#include "CameraSensor_Mt9v117_Patches.h"
/* Cam sensor register definitions */
#define CHIP_ID 0x0
#define MT9V117_CHIP_ID 0x2282
#define COMMAND_REGISTER 0x0040
#define HOST_COMMAND_OK (1 << 15)
#define HOST_COMMAND_2 (1 << 2)
#define HOST_COMMAND_1 (1 << 1)
#define HOST_COMMAND_0 (1 << 0)
#define PAD_SLEW 0x0030
#define RESET_AND_MISC_CONTROL 0x001a
#define RESET_SOC_I2C (1 << 0)
#define ACCESS_CTL_STAT 0x0982
#define PHYSICAL_ADDRESS_ACCESS 0x098a
#define LOGICAL_ADDRESS_ACCESS 0x098e
#define AE_TRACK_JUMP_DIVISOR 0xa812
#define CAM_AET_SKIP_FRAMES 0xc868
#define AE_RULE_VAR 9
#define AE_RULE_ALGO_OFFSET 4
#define AE_RULE_ALGO_AVERAGE 0
#define AE_RULE_ALGO_WEIGHTED 1
#define AE_TRACK_VAR 10
#define AWB_VAR 11
#define AWB_PIXEL_THRESHOLD_COUNT_OFFSET 64
#define LOW_LIGHT_VAR 15
#define CAM_CTRL_VAR 18
#define CAM_SENSOR_CFG_Y_ADDR_START_OFFSET 0
#define CAM_SENSOR_CFG_X_ADDR_START_OFFSET 2
#define CAM_SENSOR_CFG_Y_ADDR_END_OFFSET 4
#define CAM_SENSOR_CFG_X_ADDR_END_OFFSET 6
#define CAM_SENSOR_CFG_FRAME_LENGTH_LINES_OFFSET 14
#define CAM_SENSOR_CFG_CPIPE_LAST_ROW_OFFSET 20
#define CAM_SENSOR_CFG_FDPERIOD_60HZ 22
#define CAM_SENSOR_CFG_FDPERIOD_50HZ 24
#define CAM_SENSOR_CFG_MAX_FDZONE_60_OFFSET 26
#define CAM_SENSOR_CFG_MAX_FDZONE_50_OFFSET 28
#define CAM_SENSOR_CFG_TARGET_FDZONE_60_OFFSET 30
#define CAM_SENSOR_CFG_TARGET_FDZONE_50_OFFSET 32
#define CAM_SENSOR_CONTROL_READ_MODE_OFFSET 40
#define CAM_SENSOR_CONTROL_Y_SKIP_EN (1 << 2)
#define CAM_SENSOR_CONTROL_VERT_FLIP_EN (1 << 1)
#define CAM_SENSOR_CONTROL_HORZ_MIRROR_EN (1 << 0)
#define CAM_FLICKER_PERIOD_OFFSET 62
#define CAM_FLICKER_PERIOD_60HZ 0
#define CAM_FLICKER_PERIOD_50HZ 1
#define CAM_CROP_WINDOW_XOFFSET_OFFSET 72
#define CAM_CROP_WINDOW_YOFFSET_OFFSET 74
#define CAM_CROP_WINDOW_WIDTH_OFFSET 76
#define CAM_CROP_WINDOW_HEIGHT_OFFSET 78
#define CAM_CROP_MODE_OFFSET 80
#define CAM_OUTPUT_WIDTH_OFFSET 84
#define CAM_OUTPUT_HEIGHT_OFFSET 86
#define CAM_OUTPUT_FORMAT_OFFSET 88
#define CAM_OUTPUT_FORMAT_RGB_565 (0 << 12)
#define CAM_OUTPUT_FORMAT_RGB_555 (1 << 12)
#define CAM_OUTPUT_FORMAT_RGB_444X (2 << 12)
#define CAM_OUTPUT_FORMAT_RGB_X444 (3 << 12)
#define CAM_OUTPUT_FORMAT_BAYER_10 (0 << 10)
#define CAM_OUTPUT_FORMAT_YUV (0 << 8)
#define CAM_OUTPUT_FORMAT_RGB (1 << 8)
#define CAM_OUTPUT_FORMAT_BAYER (2 << 8)
#define CAM_OUTPUT_FORMAT_BT656_ENABLE (1 << 3)
#define CAM_OUTPUT_FORMAT_MONO_ENABLE (1 << 2)
#define CAM_OUTPUT_FORMAT_SWAP_BYTES (1 << 1)
#define CAM_OUTPUT_FORMAT_SWAP_RED_BLUE (1 << 0)
#define CAM_STAT_AWB_HG_WINDOW_XSTART_OFFSET 236
#define CAM_STAT_AWB_HG_WINDOW_YSTART_OFFSET 238
#define CAM_STAT_AWB_HG_WINDOW_XEND_OFFSET 240
#define CAM_STAT_AWB_HG_WINDOW_YEND_OFFSET 242
#define CAM_STAT_AE_INITIAL_WINDOW_XSTART_OFFSET 244
#define CAM_STAT_AE_INITIAL_WINDOW_YSTART_OFFSET 246
#define CAM_STAT_AE_INITIAL_WINDOW_XEND_OFFSET 248
#define CAM_STAT_AE_INITIAL_WINDOW_YEND_OFFSET 250
#define CAM_LL_START_GAIN_METRIC_OFFSET 278
#define CAM_LL_STOP_GAIN_METRIC_OFFSET 280
#define SYSMGR_VAR 23
#define SYSMGR_NEXT_STATE_OFFSET 0
#define PATCHLDR_VAR 24
#define PATCHLDR_LOADER_ADDRESS_OFFSET 0
#define PATCHLDR_PATCH_ID_OFFSET 2
#define PATCHLDR_FIRMWARE_ID_OFFSET 4
extern const AP_HAL::HAL& hal;
using namespace Linux;
CameraSensor_Mt9v117::CameraSensor_Mt9v117(const char *device_path,
AP_HAL::I2CDriver *i2c,
uint8_t addr,
enum mt9v117_res res,
uint16_t nrst_gpio,
uint32_t clock_freq) :
CameraSensor(device_path),
_i2c(i2c),
_addr(addr),
_nrst_gpio(nrst_gpio),
_clock_freq(clock_freq)
{
switch (res) {
case MT9V117_QVGA:
_init_sensor();
_configure_sensor_qvga();
break;
default:
AP_HAL::panic("mt9v117: unsupported resolution\n");
break;
}
_itu656_enable();
_config_change();
}
uint8_t CameraSensor_Mt9v117::_read_reg8(uint16_t reg)
{
uint8_t buf[2];
buf[0] = (uint8_t) (reg >> 8);
buf[1] = (uint8_t) (reg & 0xFF);
if (_i2c->do_transfer(_addr, buf, 2, buf, 1)) {
return buf[0];
} else {
hal.console->printf("mt9v117: error reading 0x%2x\n", reg);
return 0;
}
}
void CameraSensor_Mt9v117::_write_reg8(uint16_t reg, uint8_t val)
{
uint8_t buf[3];
buf[0] = (uint8_t) (reg >> 8);
buf[1] = (uint8_t) (reg & 0xFF);
buf[2] = val;
if (!_i2c->do_transfer(_addr, buf, 3, NULL, 0)) {
hal.console->printf("mt9v117: error writing 0x%2x\n", reg);
}
}
uint16_t CameraSensor_Mt9v117::_read_reg16(uint16_t reg)
{
uint8_t buf[2];
buf[0] = (uint8_t) (reg >> 8);
buf[1] = (uint8_t) (reg & 0xFF);
if (_i2c->do_transfer(_addr, buf, 2, buf, 2)) {
return (buf[0] << 8 | buf[1]);
} else {
hal.console->printf("mt9v117: error reading 0x%4x\n", reg);
return 0;
}
}
void CameraSensor_Mt9v117::_write_reg16(uint16_t reg, uint16_t val)
{
uint8_t buf[4];
buf[0] = (uint8_t) (reg >> 8);
buf[1] = (uint8_t) (reg & 0xFF);
buf[2] = (uint8_t) (val >> 8);
buf[3] = (uint8_t) (val & 0xFF);
if (!_i2c->do_transfer(_addr, buf, 4, NULL, 0)) {
hal.console->printf("mt9v117: error writing 0x%4x\n", reg);
}
}
void CameraSensor_Mt9v117::_write_reg32(uint16_t reg, uint32_t val)
{
uint8_t buf[6];
buf[0] = (uint8_t) (reg >> 8);
buf[1] = (uint8_t) (reg & 0xFF);
buf[2] = (uint8_t) (val >> 24);
buf[3] = (uint8_t) ((val >> 16) & 0xFF);
buf[4] = (uint8_t) ((val >> 8) & 0xFF);
buf[5] = (uint8_t) (val & 0xFF);
if (!_i2c->do_transfer(_addr, buf, 6, NULL, 0)) {
hal.console->printf("mt9v117: error writing 0x%8x\n", reg);
}
}
inline uint16_t CameraSensor_Mt9v117::_var2reg(uint16_t var,
uint16_t offset)
{
return (0x8000 | (var << 10) | offset);
}
uint16_t CameraSensor_Mt9v117::_read_var16(uint16_t var, uint16_t offset)
{
uint16_t reg = _var2reg(var, offset);
return _read_reg16(reg);
}
void CameraSensor_Mt9v117::_write_var16(uint16_t var,
uint16_t offset,
uint16_t value)
{
uint16_t reg = _var2reg(var, offset);
_write_reg16(reg, value);
}
uint8_t CameraSensor_Mt9v117::_read_var8(uint16_t var, uint16_t offset)
{
uint16_t reg = _var2reg(var, offset);
return _read_reg8(reg);
}
void CameraSensor_Mt9v117::_write_var8(uint16_t var,
uint16_t offset,
uint8_t value)
{
uint16_t reg = _var2reg(var, offset);
return _write_reg8(reg, value);
}
void CameraSensor_Mt9v117::_write_var32(uint16_t var,
uint16_t offset,
uint32_t value)
{
uint16_t reg = _var2reg(var, offset);
return _write_reg32(reg, value);
}
void CameraSensor_Mt9v117::_config_change()
{
uint16_t cmd_status;
/* timeout 100ms delay 10ms */
int timeout = 10;
_write_var8(SYSMGR_VAR, SYSMGR_NEXT_STATE_OFFSET, 0x28);
_write_reg16(COMMAND_REGISTER, HOST_COMMAND_OK | HOST_COMMAND_1);
do {
hal.scheduler->delay(10);
cmd_status = _read_reg16(COMMAND_REGISTER);
timeout--;
} while (((cmd_status & HOST_COMMAND_1) != 0) &&
(timeout > 0));
if (timeout == 0) {
hal.console->printf("mt9v117:"
"timeout waiting or command to complete\n");
}
if ((cmd_status & HOST_COMMAND_OK) == 0) {
hal.console->printf("mt9v117:config change failed\n");
}
}
void CameraSensor_Mt9v117::_itu656_enable()
{
_write_var16(CAM_CTRL_VAR, CAM_OUTPUT_FORMAT_OFFSET,
_read_var16(CAM_CTRL_VAR, CAM_OUTPUT_FORMAT_OFFSET) |
CAM_OUTPUT_FORMAT_BT656_ENABLE);
}
void CameraSensor_Mt9v117::_soft_reset()
{
_write_reg16(RESET_AND_MISC_CONTROL, RESET_SOC_I2C);
_write_reg16(RESET_AND_MISC_CONTROL, 0);
/* sleep 50ms after soft reset */
hal.scheduler->delay(50);
}
void CameraSensor_Mt9v117::_apply_patch()
{
uint16_t cmd_status;
/* timeout 100ms delay 10ms */
int timeout = 10;
/* Errata item 2 */
_write_reg16(0x301a, 0x10d0);
_write_reg16(0x31c0, 0x1404);
_write_reg16(0x3ed8, 0x879c);
_write_reg16(0x3042, 0x20e1);
_write_reg16(0x30d4, 0x8020);
_write_reg16(0x30c0, 0x0026);
_write_reg16(0x301a, 0x10d4);
/* Errata item 6 */
_write_var16(AE_TRACK_VAR, 0x0002, 0x00d3);
_write_var16(CAM_CTRL_VAR, 0x0078, 0x00a0);
_write_var16(CAM_CTRL_VAR, 0x0076, 0x0140);
/* Errata item 8 */
_write_var16(LOW_LIGHT_VAR, 0x0004, 0x00fc);
_write_var16(LOW_LIGHT_VAR, 0x0038, 0x007f);
_write_var16(LOW_LIGHT_VAR, 0x003a, 0x007f);
_write_var16(LOW_LIGHT_VAR, 0x003c, 0x007f);
_write_var16(LOW_LIGHT_VAR, 0x0004, 0x00f4);
/* Patch 0403; Critical; Sensor optimization */
_write_reg16(ACCESS_CTL_STAT, 0x0001);
_write_reg16(PHYSICAL_ADDRESS_ACCESS, 0x7000);
/* write patch */
for (unsigned int i=0; i < MT9V117_PATCH_LINE_NUM; i++) {
_i2c->do_transfer(_addr,
patch_lines[i].data, patch_lines[i].size, NULL, 0);
}
_write_reg16(LOGICAL_ADDRESS_ACCESS, 0x0000);
_write_var16(PATCHLDR_VAR, PATCHLDR_LOADER_ADDRESS_OFFSET, 0x05d8);
_write_var16(PATCHLDR_VAR, PATCHLDR_PATCH_ID_OFFSET, 0x0403);
_write_var32(PATCHLDR_VAR, PATCHLDR_FIRMWARE_ID_OFFSET, 0x00430104);
_write_reg16(COMMAND_REGISTER, HOST_COMMAND_OK | HOST_COMMAND_0);
do {
hal.scheduler->delay(10);
cmd_status = _read_reg16(COMMAND_REGISTER);
timeout--;
} while (((cmd_status & HOST_COMMAND_0) != 0) &&
(timeout > 0));
if ((cmd_status & HOST_COMMAND_OK) == 0) {
hal.console->printf("mt9v117:patch apply failed\n");
}
}
void CameraSensor_Mt9v117::_set_basic_settings()
{
_write_var32(AWB_VAR, AWB_PIXEL_THRESHOLD_COUNT_OFFSET, 50000);
_write_var16(AE_RULE_VAR, AE_RULE_ALGO_OFFSET, AE_RULE_ALGO_AVERAGE);
/* Set pixclk pad slew to 6 and data out pad slew to 1 */
_write_reg16(PAD_SLEW, _read_reg16(PAD_SLEW) | 0x0600 | 0x0001);
}
void CameraSensor_Mt9v117::_configure_sensor_qvga()
{
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_X_ADDR_START_OFFSET, 16);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_X_ADDR_END_OFFSET, 663);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_Y_ADDR_START_OFFSET, 8);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_Y_ADDR_END_OFFSET, 501);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_CPIPE_LAST_ROW_OFFSET, 243);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_FRAME_LENGTH_LINES_OFFSET, 283);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CONTROL_READ_MODE_OFFSET,
CAM_SENSOR_CONTROL_Y_SKIP_EN);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_MAX_FDZONE_60_OFFSET, 1);
_write_var16(CAM_CTRL_VAR, CAM_SENSOR_CFG_TARGET_FDZONE_60_OFFSET, 1);
_write_reg8(AE_TRACK_JUMP_DIVISOR, 0x03);
_write_reg8(CAM_AET_SKIP_FRAMES, 0x02);
_write_var16(CAM_CTRL_VAR, CAM_OUTPUT_WIDTH_OFFSET, 320);
_write_var16(CAM_CTRL_VAR, CAM_OUTPUT_HEIGHT_OFFSET, 240);
/* Set gain metric for 111.2 fps
* The final fps depends on the input clock
* (89.2fps on bebop) so a modification may be needed here */
_write_var16(CAM_CTRL_VAR, CAM_LL_START_GAIN_METRIC_OFFSET, 0x03e8);
_write_var16(CAM_CTRL_VAR, CAM_LL_STOP_GAIN_METRIC_OFFSET, 0x1770);
/* set crop window */
_write_var16(CAM_CTRL_VAR, CAM_CROP_WINDOW_XOFFSET_OFFSET, 0);
_write_var16(CAM_CTRL_VAR, CAM_CROP_WINDOW_YOFFSET_OFFSET, 0);
_write_var16(CAM_CTRL_VAR, CAM_CROP_WINDOW_WIDTH_OFFSET, 640);
_write_var16(CAM_CTRL_VAR, CAM_CROP_WINDOW_HEIGHT_OFFSET, 240);
/* Enable auto-stats mode */
_write_var8(CAM_CTRL_VAR, CAM_CROP_MODE_OFFSET, 3);
_write_var16(CAM_CTRL_VAR, CAM_STAT_AWB_HG_WINDOW_XEND_OFFSET, 319);
_write_var16(CAM_CTRL_VAR, CAM_STAT_AWB_HG_WINDOW_YEND_OFFSET, 239);
_write_var16(CAM_CTRL_VAR, CAM_STAT_AE_INITIAL_WINDOW_XSTART_OFFSET, 2);
_write_var16(CAM_CTRL_VAR, CAM_STAT_AE_INITIAL_WINDOW_YSTART_OFFSET, 2);
_write_var16(CAM_CTRL_VAR, CAM_STAT_AE_INITIAL_WINDOW_XEND_OFFSET, 65);
_write_var16(CAM_CTRL_VAR, CAM_STAT_AE_INITIAL_WINDOW_YEND_OFFSET, 49);
}
void CameraSensor_Mt9v117::_init_sensor()
{
AP_HAL::DigitalSource *gpio_source;
uint16_t id;
if (_nrst_gpio != 0xFFFF) {
gpio_source = hal.gpio->channel(_nrst_gpio);
gpio_source->mode(HAL_GPIO_OUTPUT);
gpio_source->write(1);
uint32_t delay = 3.5f + (35.0f - 3.5f) *
(54000000.0f - (float)_clock_freq) /
(54000000.0f - 6000000.0f);
hal.scheduler->delay(delay);
}
id = _read_reg16(CHIP_ID);
if (id != MT9V117_CHIP_ID) {
AP_HAL::panic("Mt9v117: bad chip id\n");
}
_soft_reset();
_apply_patch();
_set_basic_settings();
}
#endif

56
libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h Normal file
View File

@ -0,0 +1,56 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __CAMERASENSOR_MT9V117_H__
#define __CAMERASENSOR_MT9V117_H__
#include "AP_HAL_Linux.h"
#include "CameraSensor.h"
enum mt9v117_res {
MT9V117_QVGA,
};
class Linux::CameraSensor_Mt9v117 : public Linux::CameraSensor {
public:
CameraSensor_Mt9v117(const char *device_path, AP_HAL::I2CDriver *i2c,
uint8_t addr, enum mt9v117_res res,
uint16_t nrst_gpio, uint32_t clock_freq);
private:
uint8_t _read_reg8(uint16_t reg);
void _write_reg8(uint16_t reg, uint8_t val);
uint16_t _read_reg16(uint16_t reg);
void _write_reg16(uint16_t reg, uint16_t val);
void _write_reg32(uint16_t reg, uint32_t val);
inline uint16_t _var2reg(uint16_t var, uint16_t offset);
uint16_t _read_var16(uint16_t var, uint16_t offset);
void _write_var16(uint16_t var, uint16_t offset, uint16_t value);
uint8_t _read_var8(uint16_t var, uint16_t offset);
void _write_var8(uint16_t var, uint16_t offset, uint8_t value);
void _write_var32(uint16_t var, uint16_t offset, uint32_t value);
void _config_change();
void _itu656_enable();
void _soft_reset();
void _apply_patch();
void _set_basic_settings();
void _configure_sensor_qvga();
void _init_sensor();
AP_HAL::I2CDriver *_i2c;
uint8_t _addr;
uint16_t _nrst_gpio = 0xFFFF;
uint32_t _clock_freq = 0;
};
#endif

161
libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h Normal file
View File

@ -0,0 +1,161 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __CAMERASENSOR_MT9V117_PATCHES_H__
#define __CAMERASENSOR_MT9V117_PATCHES_H__
/* It seems to be mandatory to write these patches by unit of 48 bytes, the
* 2 first ones seem to be an address since it increments of 48 at each line
* Possibly easier to read if written under the form :
* writeRegisters(addr, 0xf000, 48, line1);
* ...
*/
#define MT9V117_PATCH_LINE_NUM 13
uint8_t patch_line1[] =
{
0xf0, 0x00, 0x72, 0xcf, 0xff, 0x00, 0x3e, 0xd0, 0x92, 0x00,
0x71, 0xcf, 0xff, 0xff, 0xf2, 0x18, 0xb1, 0x10, 0x92, 0x05,
0xb1, 0x11, 0x92, 0x04, 0xb1, 0x12, 0x70, 0xcf, 0xff, 0x00,
0x30, 0xc0, 0x90, 0x00, 0x7f, 0xe0, 0xb1, 0x13, 0x70, 0xcf,
0xff, 0xff, 0xe7, 0x1c, 0x88, 0x36, 0x09, 0x0f, 0x00, 0xb3
};
uint8_t patch_line2[] =
{
0xf0, 0x30, 0x69, 0x13, 0xe1, 0x80, 0xd8, 0x08, 0x20, 0xca,
0x03, 0x22, 0x71, 0xcf, 0xff, 0xff, 0xe5, 0x68, 0x91, 0x35,
0x22, 0x0a, 0x1f, 0x80, 0xff, 0xff, 0xf2, 0x18, 0x29, 0x05,
0x00, 0x3e, 0x12, 0x22, 0x11, 0x01, 0x21, 0x04, 0x0f, 0x81,
0x00, 0x00, 0xff, 0xf0, 0x21, 0x8c, 0xf0, 0x10, 0x1a, 0x22
};
uint8_t patch_line3[] =
{
0xf0, 0x60, 0x10, 0x44, 0x12, 0x20, 0x11, 0x02, 0xf7, 0x87,
0x22, 0x4f, 0x03, 0x83, 0x1a, 0x20, 0x10, 0xc4, 0xf0, 0x09,
0xba, 0xae, 0x7b, 0x50, 0x1a, 0x20, 0x10, 0x84, 0x21, 0x45,
0x01, 0xc1, 0x1a, 0x22, 0x10, 0x44, 0x70, 0xcf, 0xff, 0x00,
0x3e, 0xd0, 0xb0, 0x60, 0xb0, 0x25, 0x7e, 0xe0, 0x78, 0xe0
};
uint8_t patch_line4[] =
{
0xf0, 0x90, 0x71, 0xcf, 0xff, 0xff, 0xf2, 0x18, 0x91, 0x12,
0x72, 0xcf, 0xff, 0xff, 0xe7, 0x1c, 0x8a, 0x57, 0x20, 0x04,
0x0f, 0x80, 0x00, 0x00, 0xff, 0xf0, 0xe2, 0x80, 0x20, 0xc5,
0x01, 0x61, 0x20, 0xc5, 0x03, 0x22, 0xb1, 0x12, 0x71, 0xcf,
0xff, 0x00, 0x3e, 0xd0, 0xb1, 0x04, 0x7e, 0xe0, 0x78, 0xe0
};
uint8_t patch_line5[] =
{
0xf0, 0xc0, 0x70, 0xcf, 0xff, 0xff, 0xe7, 0x1c, 0x88, 0x57,
0x71, 0xcf, 0xff, 0xff, 0xf2, 0x18, 0x91, 0x13, 0xea, 0x84,
0xb8, 0xa9, 0x78, 0x10, 0xf0, 0x03, 0xb8, 0x89, 0xb8, 0x8c,
0xb1, 0x13, 0x71, 0xcf, 0xff, 0x00, 0x30, 0xc0, 0xb1, 0x00,
0x7e, 0xe0, 0xc0, 0xf1, 0x09, 0x1e, 0x03, 0xc0, 0xc1, 0xa1
};
uint8_t patch_line6[] =
{
0xf0, 0xf0, 0x75, 0x08, 0x76, 0x28, 0x77, 0x48, 0xc2, 0x40,
0xd8, 0x20, 0x71, 0xcf, 0x00, 0x03, 0x20, 0x67, 0xda, 0x02,
0x08, 0xae, 0x03, 0xa0, 0x73, 0xc9, 0x0e, 0x25, 0x13, 0xc0,
0x0b, 0x5e, 0x01, 0x60, 0xd8, 0x06, 0xff, 0xbc, 0x0c, 0xce,
0x01, 0x00, 0xd8, 0x00, 0xb8, 0x9e, 0x0e, 0x5a, 0x03, 0x20
};
uint8_t patch_line7[] =
{
0xf1, 0x20, 0xd9, 0x01, 0xd8, 0x00, 0xb8, 0x9e, 0x0e, 0xb6,
0x03, 0x20, 0xd9, 0x01, 0x8d, 0x14, 0x08, 0x17, 0x01, 0x91,
0x8d, 0x16, 0xe8, 0x07, 0x0b, 0x36, 0x01, 0x60, 0xd8, 0x07,
0x0b, 0x52, 0x01, 0x60, 0xd8, 0x11, 0x8d, 0x14, 0xe0, 0x87,
0xd8, 0x00, 0x20, 0xca, 0x02, 0x62, 0x00, 0xc9, 0x03, 0xe0
};
uint8_t patch_line8[] =
{
0xf1, 0x50, 0xc0, 0xa1, 0x78, 0xe0, 0xc0, 0xf1, 0x08, 0xb2,
0x03, 0xc0, 0x76, 0xcf, 0xff, 0xff, 0xe5, 0x40, 0x75, 0xcf,
0xff, 0xff, 0xe5, 0x68, 0x95, 0x17, 0x96, 0x40, 0x77, 0xcf,
0xff, 0xff, 0xe5, 0x42, 0x95, 0x38, 0x0a, 0x0d, 0x00, 0x01,
0x97, 0x40, 0x0a, 0x11, 0x00, 0x40, 0x0b, 0x0a, 0x01, 0x00
};
uint8_t patch_line9[] =
{
0xf1, 0x80, 0x95, 0x17, 0xb6, 0x00, 0x95, 0x18, 0xb7, 0x00,
0x76, 0xcf, 0xff, 0xff, 0xe5, 0x44, 0x96, 0x20, 0x95, 0x15,
0x08, 0x13, 0x00, 0x40, 0x0e, 0x1e, 0x01, 0x20, 0xd9, 0x00,
0x95, 0x15, 0xb6, 0x00, 0xff, 0xa1, 0x75, 0xcf, 0xff, 0xff,
0xe7, 0x1c, 0x77, 0xcf, 0xff, 0xff, 0xe5, 0x46, 0x97, 0x40
};
uint8_t patch_line10[] =
{
0xf1, 0xb0, 0x8d, 0x16, 0x76, 0xcf, 0xff, 0xff, 0xe5, 0x48,
0x8d, 0x37, 0x08, 0x0d, 0x00, 0x81, 0x96, 0x40, 0x09, 0x15,
0x00, 0x80, 0x0f, 0xd6, 0x01, 0x00, 0x8d, 0x16, 0xb7, 0x00,
0x8d, 0x17, 0xb6, 0x00, 0xff, 0xb0, 0xff, 0xbc, 0x00, 0x41,
0x03, 0xc0, 0xc0, 0xf1, 0x0d, 0x9e, 0x01, 0x00, 0xe8, 0x04
};
uint8_t patch_line11[] =
{
0xf1, 0xe0, 0xff, 0x88, 0xf0, 0x0a, 0x0d, 0x6a, 0x01, 0x00,
0x0d, 0x8e, 0x01, 0x00, 0xe8, 0x7e, 0xff, 0x85, 0x0d, 0x72,
0x01, 0x00, 0xff, 0x8c, 0xff, 0xa7, 0xff, 0xb2, 0xd8, 0x00,
0x73, 0xcf, 0xff, 0xff, 0xf2, 0x40, 0x23, 0x15, 0x00, 0x01,
0x81, 0x41, 0xe0, 0x02, 0x81, 0x20, 0x08, 0xf7, 0x81, 0x34
};
uint8_t patch_line12[] =
{
0xf2, 0x10, 0xa1, 0x40, 0xd8, 0x00, 0xc0, 0xd1, 0x7e, 0xe0,
0x53, 0x51, 0x30, 0x34, 0x20, 0x6f, 0x6e, 0x5f, 0x73, 0x74,
0x61, 0x72, 0x74, 0x5f, 0x73, 0x74, 0x72, 0x65, 0x61, 0x6d,
0x69, 0x6e, 0x67, 0x20, 0x25, 0x64, 0x20, 0x25, 0x64, 0x0a,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
uint8_t patch_line13[] =
{
0xf2, 0x40, 0xff, 0xff, 0xe8, 0x28, 0xff, 0xff, 0xf0, 0xe8,
0xff, 0xff, 0xe8, 0x08, 0xff, 0xff, 0xf1, 0x54
};
struct mt9v117_patch_line {
uint8_t *data;
uint8_t size;
};
struct mt9v117_patch_line patch_lines[MT9V117_PATCH_LINE_NUM] = {
{patch_line1, sizeof(patch_line1)},
{patch_line2, sizeof(patch_line2)},
{patch_line3, sizeof(patch_line3)},
{patch_line4, sizeof(patch_line4)},
{patch_line5, sizeof(patch_line5)},
{patch_line6, sizeof(patch_line6)},
{patch_line7, sizeof(patch_line7)},
{patch_line8, sizeof(patch_line8)},
{patch_line9, sizeof(patch_line9)},
{patch_line10, sizeof(patch_line10)},
{patch_line11, sizeof(patch_line11)},
{patch_line12, sizeof(patch_line12)},
{patch_line13, sizeof(patch_line13)}
};
#endif