diff --git a/libraries/AP_HAL_Linux/CameraSensor.cpp b/libraries/AP_HAL_Linux/CameraSensor.cpp
new file mode 100644
index 0000000000..3e3fdc5a86
--- /dev/null
+++ b/libraries/AP_HAL_Linux/CameraSensor.cpp
@@ -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 .
+ */
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include "CameraSensor.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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
diff --git a/libraries/AP_HAL_Linux/CameraSensor.h b/libraries/AP_HAL_Linux/CameraSensor.h
new file mode 100644
index 0000000000..21f963a514
--- /dev/null
+++ b/libraries/AP_HAL_Linux/CameraSensor.h
@@ -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 .
+ */
+#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
diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp
new file mode 100644
index 0000000000..0693d6a296
--- /dev/null
+++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp
@@ -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 .
+ */
+#include
+#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
diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h
new file mode 100644
index 0000000000..e68e132433
--- /dev/null
+++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h
@@ -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 .
+ */
+#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
diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h
new file mode 100644
index 0000000000..b3405dfd50
--- /dev/null
+++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h
@@ -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 .
+ */
+#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