From 06dfbc3e0972a02c3424743dd8a968cb35096380 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Nov 2016 14:21:28 +1100 Subject: [PATCH] AP_OpticalFlow: added common driver for PX4Flow used on Linux and NuttX boards --- .../AP_OpticalFlow/AP_OpticalFlow_Linux.cpp | 184 ------------------ .../AP_OpticalFlow/AP_OpticalFlow_Linux.h | 86 -------- .../AP_OpticalFlow/AP_OpticalFlow_PX4.cpp | 106 ---------- libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h | 20 -- .../AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp | 128 ++++++++++++ .../AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h | 48 +++++ .../AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp | 1 + libraries/AP_OpticalFlow/OpticalFlow.cpp | 14 +- libraries/AP_OpticalFlow/OpticalFlow.h | 5 +- .../AP_OpticalFlow/OpticalFlow_backend.h | 3 + 10 files changed, 193 insertions(+), 402 deletions(-) delete mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp delete mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h delete mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp delete mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp deleted file mode 100644 index 1199b52a9c..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/* - 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 . - */ - -/* - * 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_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, AP_HAL::OwnPtr dev) - : OpticalFlow_backend(_frontend) - , _dev(std::move(dev)) -{ -} - -void AP_OpticalFlow_Linux::init(void) -{ - // only initialise once - if (initialised) { - return; - } - - // take i2c bus sempahore - 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 (!_dev->read_registers(0, val, I2C_FRAME_SIZE)) { - goto fail; - } - - // success - initialised = true; - - _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Linux::timer, bool)); - -fail: - _dev->get_semaphore()->give(); -} - -bool AP_OpticalFlow_Linux::request_measurement() -{ - // send measure request to sensor - return _dev->write_register(PX4FLOW_REG, 0); -} - -bool AP_OpticalFlow_Linux::timer(void) -{ - // request measurement - request_measurement(); - - uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = {}; - i2c_integral_frame f_integral; - - // Perform the writing and reading in a single command - if (PX4FLOW_REG == 0x00) { - if (!_dev->read_registers(PX4FLOW_REG, 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 (!_dev->read_registers(PX4FLOW_REG, val, I2C_INTEGRAL_FRAME_SIZE)) { - goto fail_transfer; - } - memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); - } - - // reduce error count - if (num_errors > 0) { - num_errors--; - } - - if (_sem->take(0)) { - report.pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f; //convert to radians - report.pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f; //convert to radians - report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; - report.ground_distance_m = static_cast(f_integral.ground_distance) / 1000.0f; // convert to meters - report.quality = f_integral.qual; // 0:bad, 255 max quality - report.gyro_x_rate_integral = static_cast(f_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians - report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians - report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians - report.integration_timespan = f_integral.integration_timespan; // microseconds - report.time_since_last_sonar_update = f_integral.sonar_timestamp; // microseconds - report.gyro_temperature = f_integral.gyro_temperature; // Temperature * 100 in centi-degrees Celsius - report.sensor_id = 0; - new_report = true; - _sem->give(); - } - - return true; - -fail_transfer: - num_errors++; - return true; -} - -// update - read latest values from sensor and fill in x,y and totals. -void AP_OpticalFlow_Linux::update(void) -{ - // return immediately if not initialised or more than half of last 40 reads have failed - if (!initialised || num_errors >= 20) { - return; - } - - if (!_sem->take_nonblocking()) { - return; - } - - if (!new_report) { - _sem->give(); - return; - } - - // process - struct OpticalFlow::OpticalFlow_state state; - state.device_id = report.sensor_id; - state.surface_quality = report.quality; - if (report.integration_timespan > 0) { - const Vector2f flowScaler = _flowScaler(); - float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; - float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; - float integralToRate = 1e6f / float(report.integration_timespan); - state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis - state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis - state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis - state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis - } else { - state.flowRate.zero(); - state.bodyRate.zero(); - } - - // copy results to front end - _update_frontend(state); - - _sem->give(); - -#if PX4FLOW_DEBUG - hal.console->printf("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n", - (unsigned)state.device_id, - (unsigned)state.surface_quality, - (double)state.flowRate.x, - (double)state.flowRate.y, - (double)state.bodyRate.x, - (double)state.bodyRate.y); -#endif -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h deleted file mode 100644 index b56e8a77bd..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.h +++ /dev/null @@ -1,86 +0,0 @@ -/* - * Portions of this driver were borrowed from the PX4Firmware px4flow driver which can be found here: - * https://github.com/PX4/Firmware/blob/master/src/drivers/px4flow/px4flow.cpp - */ -#pragma once - -#include "OpticalFlow.h" - -#include -#include -#include -#include - -class AP_OpticalFlow_Linux : public OpticalFlow_backend -{ -public: - // constructor - AP_OpticalFlow_Linux(OpticalFlow &_frontend, AP_HAL::OwnPtr dev); - - // initialise the sensor - void init(); - - // read latest values from sensor and fill in x,y and totals. - void update(void); - -private: - - typedef struct PACKED { - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; - } i2c_frame; - - typedef struct PACKED { - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t sonar_timestamp; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; - uint8_t padding_not_used; - } i2c_integral_frame; - - typedef struct { - uint64_t timestamp; // in microseconds since system start - uint8_t sensor_id; // id of the sensor emitting the flow value - float pixel_flow_x_integral; // accumulated optical flow in radians around x axis - float pixel_flow_y_integral; // accumulated optical flow in radians around y axis - float gyro_x_rate_integral; // accumulated gyro value in radians around x axis - float gyro_y_rate_integral; // accumulated gyro value in radians around y axis - float gyro_z_rate_integral; // accumulated gyro value in radians around z axis - float ground_distance_m; // Altitude / distance to ground in meters - uint32_t integration_timespan; // accumulation time span in microseconds - uint32_t time_since_last_sonar_update; // time since last sonar update in microseconds - uint16_t frame_count_since_last_readout;//number of accumulated frames in time span - int16_t gyro_temperature; // Temperature * 100 in centi-degrees celsius - 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(); - - bool timer(void); - - bool initialised; - uint16_t num_errors; - - optical_flow_s report; - bool new_report; -}; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp deleted file mode 100644 index ec3cd9b13b..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* - 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 . - */ - -/* - * AP_OpticalFlow_PX4.cpp - ardupilot library for PX4Flow sensor - * - */ - -#include -#include "OpticalFlow.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include -#include -#include -#include -#include - -#include -#include - -extern const AP_HAL::HAL& hal; - -AP_OpticalFlow_PX4::AP_OpticalFlow_PX4(OpticalFlow &_frontend) : -OpticalFlow_backend(_frontend) -{} - - -extern "C" int px4flow_main(int, char **); - -void AP_OpticalFlow_PX4::init(void) -{ -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) || defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) - if (!AP_BoardConfig::px4_start_driver(px4flow_main, "px4flow", "start")) { - hal.console->printf("Unable to start px4flow driver\n"); - } else { - // give it time to initialise - hal.scheduler->delay(500); - } -#endif - _fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); - - // check for failure to open device - if (_fd == -1) { - return; - } - - // change to 10Hz update - if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) { - hal.console->printf("Unable to set flow rate to 10Hz\n"); - } -} - -// update - read latest values from sensor and fill in x,y and totals. -void AP_OpticalFlow_PX4::update(void) -{ - // return immediately if not initialised - if (_fd == -1) { - return; - } - - struct optical_flow_s report; - while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && - report.timestamp != _last_timestamp) { - struct OpticalFlow::OpticalFlow_state state; - state.device_id = report.sensor_id; - state.surface_quality = report.quality; - if (report.integration_timespan > 0) { - const Vector2f flowScaler = _flowScaler(); - float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; - float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; - float integralToRate = 1e6f / float(report.integration_timespan); - - // rad/sec measured optically about the X body axis - state.flowRate.x = flowScaleFactorX * integralToRate * report.pixel_flow_x_integral; - state.flowRate.y = flowScaleFactorY * integralToRate * report.pixel_flow_y_integral; - _applyYaw(state.flowRate); - - // rad/sec measured inertially about the X body axis - state.bodyRate.x = integralToRate * report.gyro_x_rate_integral; - state.bodyRate.y = integralToRate * report.gyro_y_rate_integral; - _applyYaw(state.bodyRate); - } else { - state.flowRate.zero(); - state.bodyRate.zero(); - } - _last_timestamp = report.timestamp; - - _update_frontend(state); - } -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h deleted file mode 100644 index a05235b9c8..0000000000 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include "OpticalFlow.h" - -class AP_OpticalFlow_PX4 : public OpticalFlow_backend -{ -public: - /// constructor - AP_OpticalFlow_PX4(OpticalFlow &_frontend); - - // init - initialise the sensor - void init(); - - // update - read latest values from sensor and fill in x,y and totals. - void update(void); - -private: - int _fd; // file descriptor for sensor - uint64_t _last_timestamp; // time of last update (used to avoid processing old reports) -}; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp new file mode 100644 index 0000000000..a4386d7f8c --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp @@ -0,0 +1,128 @@ +/* + 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 . + */ +/* + driver for PX4Flow optical flow sensor + */ + +#include +#include "AP_OpticalFlow_PX4Flow.h" +#include +#include +#include +#include "OpticalFlow.h" +#include + +extern const AP_HAL::HAL& hal; + +#define PX4FLOW_BASE_I2C_ADDR 0x42 + +// constructor +AP_OpticalFlow_PX4Flow::AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend) : + OpticalFlow_backend(_frontend) +{ +} + + +// detect the device +AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(OpticalFlow &_frontend) +{ + AP_OpticalFlow_PX4Flow *sensor = new AP_OpticalFlow_PX4Flow(_frontend); + if (!sensor) { + return nullptr; + } + if (!sensor->setup_sensor()) { + delete sensor; + return nullptr; + } + return sensor; +} + +/* + look for the sensor on different buses + */ +bool AP_OpticalFlow_PX4Flow::scan_buses(void) +{ + for (uint8_t bus=0; bus<2; bus++) { +#ifdef HAL_OPTFLOW_PX4FLOW_I2C_BUS + // only one bus from HAL + if (bus != HAL_OPTFLOW_PX4FLOW_I2C_BUS) { + continue; + } +#endif + AP_HAL::OwnPtr tdev = hal.i2c_mgr->get_device(bus, PX4FLOW_BASE_I2C_ADDR + get_bus_id()); + if (!tdev) { + continue; + } + if (!tdev->get_semaphore()->take(0)) { + continue; + } + struct i2c_integral_frame frame; + bool ok = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame)); + tdev->get_semaphore()->give(); + if (ok) { + printf("Found PX4Flow on bus %u\n", bus); + dev = std::move(tdev); + break; + } + } + return !!dev; +} + +// setup the device +bool AP_OpticalFlow_PX4Flow::setup_sensor(void) +{ + if (!scan_buses()) { + return false; + } + // read at 10Hz + dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_PX4Flow::timer, bool)); + return true; +} + + +// update - read latest values from sensor and fill in x,y and totals. +void AP_OpticalFlow_PX4Flow::update(void) +{ +} + +// timer to read sensor +bool AP_OpticalFlow_PX4Flow::timer(void) +{ + struct i2c_integral_frame frame; + if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) { + return true; + } + struct OpticalFlow::OpticalFlow_state state {}; + state.device_id = get_bus_id(); + + if (frame.integration_timespan > 0) { + const Vector2f flowScaler = _flowScaler(); + float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; + float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; + float integralToRate = 1.0e6 / frame.integration_timespan; + + state.surface_quality = frame.qual; + state.flowRate = Vector2f(frame.pixel_flow_x_integral * flowScaleFactorX, + frame.pixel_flow_y_integral * flowScaleFactorY) * 1.0e-4 * integralToRate; + state.bodyRate = Vector2f(frame.gyro_x_rate_integral, frame.gyro_y_rate_integral) * 1.0e-4 * integralToRate; + + _applyYaw(state.flowRate); + _applyYaw(state.bodyRate); + } + + _update_frontend(state); + + return true; +} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h new file mode 100644 index 0000000000..f3a03eeea5 --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h @@ -0,0 +1,48 @@ +#pragma once + +#include "OpticalFlow.h" +#include + +class AP_OpticalFlow_PX4Flow : public OpticalFlow_backend +{ +public: + /// constructor + AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend); + + // init - initialise the sensor + void init() override {} + + // update - read latest values from sensor and fill in x,y and totals. + void update(void) override; + + // detect if the sensor is available + static AP_OpticalFlow_PX4Flow *detect(OpticalFlow &_frontend); + +private: + AP_HAL::OwnPtr dev; + + static const uint8_t REG_INTEGRAL_FRAME = 0x16; + + // I2C data on register REG_INTEGRAL_FRAME + struct PACKED i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; + }; + + // scan I2C bus addresses and buses + bool scan_buses(void); + + // setup sensor + bool setup_sensor(void); + + bool timer(void); +}; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp index c9b6c69d9e..fc520a863d 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp @@ -25,6 +25,7 @@ #include #include #include "OpticalFlow.h" +#include "AP_OpticalFlow_Pixart.h" #include "AP_OpticalFlow_Pixart_SROM.h" #include diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 1233acf69d..d3189bae75 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -1,6 +1,9 @@ #include #include "OpticalFlow.h" #include "AP_OpticalFlow_Onboard.h" +#include "AP_OpticalFlow_SITL.h" +#include "AP_OpticalFlow_Pixart.h" +#include "AP_OpticalFlow_PX4Flow.h" extern const AP_HAL::HAL& hal; @@ -55,6 +58,13 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = { // @User: Advanced AP_GROUPINFO("_POS", 4, OpticalFlow, _pos_offset, 0.0f), + // @Param: _BUS_ID + // @DisplayName: ID on the bus + // @Description: This is used to select between multiple possible bus IDs for some sensor types. For PX4Flow you can choose 0 to 7 for the 8 possible addresses on the I2C bus. + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("_BUS_ID", 5, OpticalFlow, _bus_id, 0), + AP_GROUPEND }; @@ -80,7 +90,7 @@ void OpticalFlow::init(void) backend = AP_OpticalFlow_Pixart::detect(*this); } if (backend == nullptr) { - backend = new AP_OpticalFlow_PX4(*this); + backend = AP_OpticalFlow_PX4Flow::detect(*this); } #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL backend = new AP_OpticalFlow_SITL(*this); @@ -89,7 +99,7 @@ void OpticalFlow::init(void) CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI backend = new AP_OpticalFlow_Onboard(*this); #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - backend = new AP_OpticalFlow_Linux(*this, hal.i2c_mgr->get_device(HAL_OPTFLOW_PX4FLOW_I2C_BUS, HAL_OPTFLOW_PX4FLOW_I2C_ADDRESS)); + backend = AP_OpticalFlow_PX4Flow::detect(*this); #endif } diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index 4955675a82..0c1fbe87b9 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -89,6 +89,7 @@ private: AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand AP_Int16 _yawAngle_cd; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees AP_Vector3f _pos_offset; // position offset of the flow sensor in the body frame + AP_Int8 _bus_id; // ID on bus (some sensors only) // state filled in by backend struct OpticalFlow_state _state; @@ -97,7 +98,3 @@ private: }; #include "OpticalFlow_backend.h" -#include "AP_OpticalFlow_SITL.h" -#include "AP_OpticalFlow_PX4.h" -#include "AP_OpticalFlow_Linux.h" -#include "AP_OpticalFlow_Pixart.h" diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.h b/libraries/AP_OpticalFlow/OpticalFlow_backend.h index e4e0609363..fae7283d88 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow_backend.h +++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.h @@ -53,6 +53,9 @@ protected: // get access to AHRS object AP_AHRS_NavEKF &get_ahrs(void) { return frontend._ahrs; } + + // get bus ID parameter + uint8_t get_bus_id(void) const { return frontend._bus_id; } // semaphore for access to shared frontend data AP_HAL::Semaphore *_sem;