mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
AP_HAL_Linux: make CameraSensor follow coding style
- Replace tabs with spaces - Sort includes - No need to ifdef Linux inside AP_HAL_Linux - Use early returns on error rather than a chain o if/else - Use pragma once - No need to initialize class members to 0, it's already our default behavior
This commit is contained in:
parent
c84d9bf702
commit
24d20f89c4
@ -12,43 +12,40 @@
|
||||
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 <fcntl.h>
|
||||
#include <linux/v4l2-subdev.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.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;
|
||||
int ret, fd;
|
||||
|
||||
fd = open(_device_path, O_RDWR);
|
||||
if (fd < 0) {
|
||||
return false;
|
||||
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;
|
||||
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;
|
||||
ret = ioctl(fd, VIDIOC_SUBDEV_S_FMT, &fmt);
|
||||
if (ret < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
@ -12,18 +12,17 @@
|
||||
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__
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_Linux.h"
|
||||
|
||||
class Linux::CameraSensor {
|
||||
public:
|
||||
CameraSensor(const char *device_path) {_device_path = device_path;}
|
||||
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
|
||||
|
@ -111,12 +111,12 @@ CameraSensor_Mt9v117::CameraSensor_Mt9v117(const char *device_path,
|
||||
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)
|
||||
uint32_t clock_freq)
|
||||
: CameraSensor(device_path)
|
||||
, _i2c(i2c)
|
||||
, _addr(addr)
|
||||
, _nrst_gpio(nrst_gpio)
|
||||
, _clock_freq(clock_freq)
|
||||
{
|
||||
switch (res) {
|
||||
case MT9V117_QVGA:
|
||||
@ -138,12 +138,12 @@ 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)) {
|
||||
return buf[0];
|
||||
} else {
|
||||
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)
|
||||
@ -164,12 +164,12 @@ 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)) {
|
||||
return (buf[0] << 8 | buf[1]);
|
||||
} else {
|
||||
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)
|
||||
@ -316,7 +316,7 @@ void CameraSensor_Mt9v117::_apply_patch()
|
||||
_write_reg16(PHYSICAL_ADDRESS_ACCESS, 0x7000);
|
||||
|
||||
/* write patch */
|
||||
for (unsigned int i=0; i < MT9V117_PATCH_LINE_NUM; i++) {
|
||||
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);
|
||||
}
|
||||
|
@ -12,8 +12,7 @@
|
||||
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__
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_Linux.h"
|
||||
#include "CameraSensor.h"
|
||||
@ -27,6 +26,7 @@ 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);
|
||||
@ -48,9 +48,7 @@ private:
|
||||
void _init_sensor();
|
||||
|
||||
AP_HAL::I2CDriver *_i2c;
|
||||
uint8_t _addr;
|
||||
uint32_t _clock_freq;
|
||||
uint16_t _nrst_gpio = 0xFFFF;
|
||||
uint32_t _clock_freq = 0;
|
||||
uint8_t _addr;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -12,12 +12,11 @@
|
||||
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__
|
||||
#pragma once
|
||||
|
||||
/* 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 :
|
||||
* Possibly easier to read if written under the form :
|
||||
* writeRegisters(addr, 0xf000, 48, line1);
|
||||
* ...
|
||||
*/
|
||||
@ -157,5 +156,3 @@ struct mt9v117_patch_line patch_lines[MT9V117_PATCH_LINE_NUM] = {
|
||||
{patch_line12, sizeof(patch_line12)},
|
||||
{patch_line13, sizeof(patch_line13)}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
Loading…
Reference in New Issue
Block a user