Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   driver for PX4Flow optical flow sensor
      17             :  */
      18             : 
      19             : #include "AP_OpticalFlow_config.h"
      20             : 
      21             : #if AP_OPTICALFLOW_PX4FLOW_ENABLED
      22             : 
      23             : #include "AP_OpticalFlow_PX4Flow.h"
      24             : 
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_Math/crc.h>
      27             : #include <AP_AHRS/AP_AHRS.h>
      28             : #include <AP_HAL/I2CDevice.h>
      29             : #include <utility>
      30             : #include "AP_OpticalFlow.h"
      31             : #include <stdio.h>
      32             : #include <AP_BoardConfig/AP_BoardConfig.h>
      33             : 
      34             : extern const AP_HAL::HAL& hal;
      35             : 
      36             : #define PX4FLOW_BASE_I2C_ADDR   0x42
      37             : #define PX4FLOW_INIT_RETRIES    10      // attempt to initialise the sensor up to 10 times at startup
      38             : 
      39             : 
      40             : // detect the device
      41           0 : AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(AP_OpticalFlow &_frontend)
      42             : {
      43           0 :     AP_OpticalFlow_PX4Flow *sensor = NEW_NOTHROW AP_OpticalFlow_PX4Flow(_frontend);
      44           0 :     if (!sensor) {
      45           0 :         return nullptr;
      46             :     }
      47           0 :     if (!sensor->setup_sensor()) {
      48           0 :         delete sensor;
      49           0 :         return nullptr;
      50             :     }
      51           0 :     return sensor;
      52             : }
      53             : 
      54             : /*
      55             :   look for the sensor on different buses
      56             :  */
      57           0 : bool AP_OpticalFlow_PX4Flow::scan_buses(void)
      58             : {
      59           0 :     bool success = false;
      60           0 :     uint8_t retry_attempt = 0;
      61             : 
      62           0 :     while (!success && retry_attempt < PX4FLOW_INIT_RETRIES) {
      63           0 :         bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
      64           0 :         uint32_t bus_mask = all_external? hal.i2c_mgr->get_bus_mask() : hal.i2c_mgr->get_bus_mask_external();
      65           0 :         FOREACH_I2C_MASK(bus, bus_mask) {
      66             :     #ifdef HAL_OPTFLOW_PX4FLOW_I2C_BUS
      67             :             // only one bus from HAL
      68           0 :             if (bus != HAL_OPTFLOW_PX4FLOW_I2C_BUS) {
      69           0 :                 continue;
      70             :             }
      71             :     #endif
      72           0 :             AP_HAL::OwnPtr<AP_HAL::Device> tdev = hal.i2c_mgr->get_device(bus, PX4FLOW_BASE_I2C_ADDR + get_address());
      73           0 :             if (!tdev) {
      74           0 :                 continue;
      75             :             }
      76           0 :             WITH_SEMAPHORE(tdev->get_semaphore());
      77             : 
      78             :             struct i2c_integral_frame frame;
      79           0 :             success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
      80           0 :             if (success) {
      81           0 :                 printf("Found PX4Flow on bus %u\n", unsigned(bus));
      82           0 :                 dev = std::move(tdev);
      83           0 :                 break;
      84             :             }
      85           0 :         }
      86           0 :         retry_attempt++;
      87           0 :         if (!success) {
      88           0 :             hal.scheduler->delay(10);
      89             :         }
      90             :     }
      91           0 :     return success;
      92             : }
      93             : 
      94             : // setup the device
      95           0 : bool AP_OpticalFlow_PX4Flow::setup_sensor(void)
      96             : {
      97           0 :     if (!scan_buses()) {
      98           0 :         return false;
      99             :     }
     100             :     // read at 10Hz
     101           0 :     dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_PX4Flow::timer, void));
     102           0 :     return true;
     103             : }
     104             : 
     105             : 
     106             : // update - read latest values from sensor and fill in x,y and totals.
     107           0 : void AP_OpticalFlow_PX4Flow::update(void)
     108             : {
     109           0 : }
     110             : 
     111             : // timer to read sensor
     112           0 : void AP_OpticalFlow_PX4Flow::timer(void)
     113             : {
     114             :     struct i2c_integral_frame frame;
     115           0 :     if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) {
     116           0 :         return;
     117             :     }
     118           0 :     struct AP_OpticalFlow::OpticalFlow_state state {};
     119             : 
     120           0 :     if (frame.integration_timespan > 0) {
     121           0 :         const Vector2f flowScaler = _flowScaler();
     122           0 :         float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
     123           0 :         float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
     124           0 :         float integralToRate = 1.0e6 / frame.integration_timespan;
     125             :         
     126           0 :         state.surface_quality = frame.qual;
     127           0 :         state.flowRate = Vector2f(frame.pixel_flow_x_integral * flowScaleFactorX,
     128           0 :                                   frame.pixel_flow_y_integral * flowScaleFactorY) * 1.0e-4 * integralToRate;
     129           0 :         state.bodyRate = Vector2f(frame.gyro_x_rate_integral, frame.gyro_y_rate_integral) * 1.0e-4 * integralToRate;
     130             :         
     131           0 :         _applyYaw(state.flowRate);
     132           0 :         _applyYaw(state.bodyRate);
     133             :     }
     134             : 
     135           0 :     _update_frontend(state);
     136             : }
     137             : 
     138             : #endif  // AP_OPTICALFLOW_PX4FLOW_ENABLED