Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp

420 lines
15 KiB
C++
Raw Permalink Blame History

/*
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_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#include "CameraSensor_Mt9v117.h"
#include <utility>
#include "GPIO.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::OwnPtr<AP_HAL::I2CDevice> dev,
enum mt9v117_res res,
uint16_t nrst_gpio, uint32_t clock_freq)
: CameraSensor(device_path)
, _dev(std::move(dev))
, _clock_freq(clock_freq)
, _nrst_gpio(nrst_gpio)
{
if (!_dev) {
AP_HAL::panic("Could not find I2C bus for CameraSensor_Mt9v117");
}
switch (res) {
case MT9V117_QVGA:
_init_sensor();
_configure_sensor_qvga();
break;
default:
AP_HAL::panic("mt9v117: unsupported resolution");
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 (!_dev->transfer(buf, 2, buf, 1)) {
hal.console->printf("mt9v117: error reading 0x%2x\n", reg);
return 0;
}
return buf[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 (!_dev->transfer(buf, 3, nullptr, 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 (!_dev->transfer(buf, 2, buf, 2)) {
hal.console->printf("mt9v117: error reading 0x%4x\n", reg);
return 0;
}
return (buf[0] << 8 | buf[1]);
}
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 (!_dev->transfer(buf, 4, nullptr, 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 (!_dev->transfer(buf, 6, nullptr, 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++) {
_dev->transfer(_patch_lines[i].data, _patch_lines[i].size, nullptr, 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 0x%04x", id);
}
_soft_reset();
_apply_patch();
_set_basic_settings();
}
#endif
Reference in New Issue View Git Blame Copy Permalink