From 84f45f09a3e2a81621d1c8b99da68fa65f8cc545 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Fri, 22 Jul 2016 14:34:27 -0300 Subject: [PATCH] AP_HAL_Linux: convert CameraSensor_Mt9v117 to I2CDevice --- .../AP_HAL_Linux/CameraSensor_Mt9v117.cpp | 30 ++++++++++--------- libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h | 9 ++++-- .../AP_HAL_Linux/OpticalFlow_Onboard.cpp | 3 +- 3 files changed, 24 insertions(+), 18 deletions(-) diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp index 492bcfa708..a36c432e83 100644 --- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp +++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp @@ -15,8 +15,10 @@ #include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP - #include "CameraSensor_Mt9v117.h" + +#include + #include "GPIO.h" /* Cam sensor register definitions */ @@ -106,17 +108,18 @@ extern const AP_HAL::HAL& hal; using namespace Linux; CameraSensor_Mt9v117::CameraSensor_Mt9v117(const char *device_path, - AP_HAL::I2CDriver *i2c, - uint8_t addr, + AP_HAL::OwnPtr dev, enum mt9v117_res res, - uint16_t nrst_gpio, - uint32_t clock_freq) + uint16_t nrst_gpio, uint32_t clock_freq) : CameraSensor(device_path) - , _i2c(i2c) - , _addr(addr) + , _dev(std::move(dev)) , _nrst_gpio(nrst_gpio) , _clock_freq(clock_freq) { + if (!_dev) { + AP_HAL::panic("Could not find I2C bus for CameraSensor_Mt9v117"); + } + switch (res) { case MT9V117_QVGA: _init_sensor(); @@ -137,7 +140,7 @@ uint8_t CameraSensor_Mt9v117::_read_reg8(uint16_t reg) buf[0] = (uint8_t) (reg >> 8); buf[1] = (uint8_t) (reg & 0xFF); - if (!_i2c->do_transfer(_addr, buf, 2, buf, 1)) { + if (!_dev->transfer(buf, 2, buf, 1)) { hal.console->printf("mt9v117: error reading 0x%2x\n", reg); return 0; } @@ -152,7 +155,7 @@ void CameraSensor_Mt9v117::_write_reg8(uint16_t reg, uint8_t val) buf[1] = (uint8_t) (reg & 0xFF); buf[2] = val; - if (!_i2c->do_transfer(_addr, buf, 3, NULL, 0)) { + if (!_dev->transfer(buf, 3, nullptr, 0)) { hal.console->printf("mt9v117: error writing 0x%2x\n", reg); } } @@ -163,7 +166,7 @@ uint16_t CameraSensor_Mt9v117::_read_reg16(uint16_t reg) buf[0] = (uint8_t) (reg >> 8); buf[1] = (uint8_t) (reg & 0xFF); - if (!_i2c->do_transfer(_addr, buf, 2, buf, 2)) { + if (!_dev->transfer(buf, 2, buf, 2)) { hal.console->printf("mt9v117: error reading 0x%4x\n", reg); return 0; } @@ -179,7 +182,7 @@ void CameraSensor_Mt9v117::_write_reg16(uint16_t reg, uint16_t val) buf[2] = (uint8_t) (val >> 8); buf[3] = (uint8_t) (val & 0xFF); - if (!_i2c->do_transfer(_addr, buf, 4, NULL, 0)) { + if (!_dev->transfer(buf, 4, nullptr, 0)) { hal.console->printf("mt9v117: error writing 0x%4x\n", reg); } } @@ -194,7 +197,7 @@ void CameraSensor_Mt9v117::_write_reg32(uint16_t reg, uint32_t val) buf[4] = (uint8_t) ((val >> 8) & 0xFF); buf[5] = (uint8_t) (val & 0xFF); - if (!_i2c->do_transfer(_addr, buf, 6, NULL, 0)) { + if (!_dev->transfer(buf, 6, NULL, 0)) { hal.console->printf("mt9v117: error writing 0x%8x\n", reg); } } @@ -316,8 +319,7 @@ void CameraSensor_Mt9v117::_apply_patch() /* 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); + _dev->transfer(_patch_lines[i].data, _patch_lines[i].size, nullptr, 0); } _write_reg16(LOGICAL_ADDRESS_ACCESS, 0x0000); diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h index 2b85e556d5..b5c2706603 100644 --- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h +++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h @@ -14,6 +14,8 @@ */ #pragma once +#include + #include "AP_HAL_Linux.h" #include "CameraSensor.h" @@ -32,8 +34,9 @@ struct mt9v117_patch { class CameraSensor_Mt9v117 : public CameraSensor { public: - CameraSensor_Mt9v117(const char *device_path, AP_HAL::I2CDriver *i2c, - uint8_t addr, enum mt9v117_res res, + CameraSensor_Mt9v117(const char *device_path, + AP_HAL::OwnPtr dev, + enum mt9v117_res res, uint16_t nrst_gpio, uint32_t clock_freq); private: @@ -58,7 +61,7 @@ private: void _configure_sensor_qvga(); void _init_sensor(); - AP_HAL::I2CDriver *_i2c; + AP_HAL::OwnPtr _dev; uint32_t _clock_freq; uint16_t _nrst_gpio = 0xFFFF; uint8_t _addr; diff --git a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp index 83a1cb12b7..5de459aa01 100644 --- a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp +++ b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp @@ -80,7 +80,8 @@ void OpticalFlow_Onboard::init(AP_HAL::OpticalFlow::Gyro_Cb get_gyro) _pwm->enable(true); _camerasensor = new CameraSensor_Mt9v117(HAL_OPTFLOW_ONBOARD_SUBDEV_PATH, - hal.i2c, 0x5D, MT9V117_QVGA, + hal.i2c_mgr->get_device(0, 0x5D), + MT9V117_QVGA, BEBOP_GPIO_CAMV_NRST, BEBOP_CAMV_PWM_FREQ); if (!_camerasensor->set_format(HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH,