mirror of https://github.com/ArduPilot/ardupilot
419 lines
15 KiB
C++
419 lines
15 KiB
C++
/*
|
|
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)) {
|
|
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 (!_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)) {
|
|
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 (!_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
|