From 46fb57fcf116fef9dbb2fa7b3d2171934841152e Mon Sep 17 00:00:00 2001 From: Ricardo de Almeida Gonzaga Date: Fri, 1 Apr 2016 18:11:18 -0300 Subject: [PATCH] AP_OpticalFlow: use I2CDevice interface --- libraries/AP_HAL/AP_HAL_Boards.h | 3 ++ .../AP_OpticalFlow/AP_OpticalFlow_Linux.cpp | 51 ++++++++----------- .../AP_OpticalFlow/AP_OpticalFlow_Linux.h | 6 ++- libraries/AP_OpticalFlow/OpticalFlow.cpp | 2 +- 4 files changed, 29 insertions(+), 33 deletions(-) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 92646832d8..500cadbee0 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -427,6 +427,9 @@ #define HAL_LINUX_UARTS_ON_TIMER_THREAD 0 #endif +#ifndef HAL_OPTFLOW_PX4FLOW_I2C_ADDRESS +#define HAL_OPTFLOW_PX4FLOW_I2C_ADDRESS 0x42 +#endif #elif CONFIG_HAL_BOARD == HAL_BOARD_EMPTY #define HAL_BOARD_NAME "EMPTY" diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp index fbf77d3461..468ad7adbd 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp @@ -17,27 +17,31 @@ /* * AP_OpticalFlow_Linux.cpp - ardupilot library for the PX4Flow sensor. * inspired by the PX4Firmware code. - * + * * @author: Víctor Mayoral Vilches * + * Address range 0x42 - 0x49 */ +#include + #include + #include "OpticalFlow.h" #define PX4FLOW_DEBUG 1 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#define PX4FLOW_I2C_ADDRESS 0x42 // 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 #define PX4FLOW_REG 0x16 // Measure Register 22 #define I2C_FRAME_SIZE (sizeof(i2c_frame)) #define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) extern const AP_HAL::HAL& hal; -AP_OpticalFlow_Linux::AP_OpticalFlow_Linux(OpticalFlow &_frontend) : - OpticalFlow_backend(_frontend) +AP_OpticalFlow_Linux::AP_OpticalFlow_Linux(OpticalFlow &_frontend, AP_HAL::OwnPtr dev) + : OpticalFlow_backend(_frontend) + , _dev(std::move(dev)) {} void AP_OpticalFlow_Linux::init(void) @@ -47,50 +51,35 @@ void AP_OpticalFlow_Linux::init(void) return; } - // get pointer to i2c bus semaphore - AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore(); - if (i2c_sem == NULL) { - return; - } - // take i2c bus sempahore - if (!i2c_sem->take(200)) { + if (!_dev->get_semaphore()->take(200)) { return; } // read from flow sensor to ensure it is not a ll40ls Lidar (which can be on 0x42) // read I2C_FRAME_SIZE bytes, the ll40ls will error whereas the flow happily returns data uint8_t val[I2C_FRAME_SIZE]; - if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE, val) != 0) { - i2c_sem->give(); - return; + if (!_dev->read_registers(0, val, I2C_FRAME_SIZE)) { + goto fail; } // success initialised = true; - i2c_sem->give(); + +fail: + _dev->get_semaphore()->give(); } bool AP_OpticalFlow_Linux::request_measurement() { // send measure request to sensor - uint8_t cmd = PX4FLOW_REG; - if (hal.i2c->writeRegisters(PX4FLOW_I2C_ADDRESS, cmd, 0, nullptr) != 0) { - return false; - } - return true; + return _dev->write_register(PX4FLOW_REG, 0); } bool AP_OpticalFlow_Linux::read(optical_flow_s* report) { - // get pointer to i2c bus semaphore - AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore(); - if (i2c_sem == NULL) { - return false; - } - // take i2c bus sempahore (non blocking) - if (!i2c_sem->take_nonblocking()) { + if (!_dev->get_semaphore()->take_nonblocking()) { return false; } @@ -102,20 +91,20 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report) // Perform the writing and reading in a single command if (PX4FLOW_REG == 0x00) { - if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE, val) != 0) { + if (!_dev->read_registers(0, val, I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE)) { goto fail_transfer; } memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - if (hal.i2c->readRegisters(PX4FLOW_I2C_ADDRESS, 0, I2C_INTEGRAL_FRAME_SIZE, val) != 0) { + if (!_dev->read_registers(0, val, I2C_INTEGRAL_FRAME_SIZE)) { goto fail_transfer; } memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } - i2c_sem->give(); + _dev->get_semaphore()->give(); // reduce error count if (num_errors > 0) { @@ -139,7 +128,7 @@ bool AP_OpticalFlow_Linux::read(optical_flow_s* report) fail_transfer: num_errors++; - i2c_sem->give(); + _dev->get_semaphore()->give(); return false; } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h index fd9f561686..9e5e24ca85 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h @@ -9,13 +9,15 @@ #include "OpticalFlow.h" #include +#include +#include #include class AP_OpticalFlow_Linux : public OpticalFlow_backend { public: // constructor - AP_OpticalFlow_Linux(OpticalFlow &_frontend); + AP_OpticalFlow_Linux(OpticalFlow &_frontend, AP_HAL::OwnPtr dev); // initialise the sensor void init(); @@ -70,6 +72,8 @@ private: uint8_t quality; // Average of quality of accumulated frames, 0: bad quality, 255: maximum quality } optical_flow_s; + AP_HAL::OwnPtr _dev; + // request the sensor produce a measurement, returns true on success bool request_measurement(); diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 979996e145..c79ad31a03 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -51,7 +51,7 @@ OpticalFlow::OpticalFlow(AP_AHRS_NavEKF& ahrs) : CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI backend(new AP_OpticalFlow_Onboard(*this, ahrs)), #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - backend(new AP_OpticalFlow_Linux(*this)), + backend(new AP_OpticalFlow_Linux(*this, hal.i2c_mgr->get_device(1, HAL_OPTFLOW_PX4FLOW_I2C_ADDRESS))), #else backend(NULL), #endif