diff --git a/libraries/AP_HAL_Linux/CameraSensor.cpp b/libraries/AP_HAL_Linux/CameraSensor.cpp
index 3e3fdc5a86..568b0dfee8 100644
--- a/libraries/AP_HAL_Linux/CameraSensor.cpp
+++ b/libraries/AP_HAL_Linux/CameraSensor.cpp
@@ -12,43 +12,40 @@
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
#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;
+ 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
diff --git a/libraries/AP_HAL_Linux/CameraSensor.h b/libraries/AP_HAL_Linux/CameraSensor.h
index 21f963a514..f5858aa8a0 100644
--- a/libraries/AP_HAL_Linux/CameraSensor.h
+++ b/libraries/AP_HAL_Linux/CameraSensor.h
@@ -12,18 +12,17 @@
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__
+#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
diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp
index 0693d6a296..f54d8f75c0 100644
--- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp
+++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp
@@ -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);
}
diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h
index e68e132433..410782f2ae 100644
--- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h
+++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.h
@@ -12,8 +12,7 @@
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__
+#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
diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h
index b3405dfd50..301e9344ad 100644
--- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h
+++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117_Patches.h
@@ -12,12 +12,11 @@
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__
+#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