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             : /*
      17             :    driver for Cheerson CX-OF optical flow sensor
      18             : 
      19             :    CXOF serial packet description
      20             :    byte0: header (0xFE)
      21             :    byte1: reserved
      22             :    byte2: x-motion low byte;
      23             :    byte3: x-motion high byte;
      24             :    byte4: y-motion low byte;
      25             :    byte5: y-motion high byte;
      26             :    byte6: t-motion
      27             :    byte7: surface quality
      28             :    byte8: footer (0xAA)
      29             : 
      30             :    sensor sends packets at 25hz
      31             :  */
      32             : 
      33             : #include "AP_OpticalFlow_CXOF.h"
      34             : 
      35             : #if AP_OPTICALFLOW_CXOF_ENABLED
      36             : 
      37             : #include <AP_HAL/AP_HAL.h>
      38             : #include <AP_Math/crc.h>
      39             : #include <AP_AHRS/AP_AHRS.h>
      40             : #include <AP_SerialManager/AP_SerialManager.h>
      41             : #include <utility>
      42             : #include "AP_OpticalFlow.h"
      43             : #include <stdio.h>
      44             : 
      45             : #define CXOF_HEADER         (uint8_t)0xFE
      46             : #define CXOF_FOOTER         (uint8_t)0xAA
      47             : #define CXOF_FRAME_LENGTH               9
      48             : #define CXOF_PIXEL_SCALING      (1.76e-3)
      49             : #define CXOF_TIMEOUT_SEC             0.3f
      50             : 
      51             : extern const AP_HAL::HAL& hal;
      52             : 
      53             : // constructor
      54           0 : AP_OpticalFlow_CXOF::AP_OpticalFlow_CXOF(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) :
      55             :     OpticalFlow_backend(_frontend),
      56           0 :     uart(_uart)
      57             : {
      58           0 : }
      59             : 
      60             : // detect the device
      61           0 : AP_OpticalFlow_CXOF *AP_OpticalFlow_CXOF::detect(AP_OpticalFlow &_frontend)
      62             : {
      63           0 :     AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
      64           0 :     if (serial_manager == nullptr) {
      65           0 :         return nullptr;
      66             :     }
      67             : 
      68             :     // look for first serial driver with protocol defined as OpticalFlow
      69           0 :     AP_HAL::UARTDriver *uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_OpticalFlow, 0);
      70           0 :     if (uart == nullptr) {
      71           0 :         return nullptr;
      72             :     }
      73             : 
      74             :     // we have found a serial port so use it
      75           0 :     AP_OpticalFlow_CXOF *sensor = NEW_NOTHROW AP_OpticalFlow_CXOF(_frontend, uart);
      76           0 :     return sensor;
      77             : }
      78             : 
      79             : // initialise the sensor
      80           0 : void AP_OpticalFlow_CXOF::init()
      81             : {
      82             :     // sanity check uart
      83           0 :     if (uart == nullptr) {
      84           0 :         return;
      85             :     }
      86             : 
      87             :     // open serial port with baud rate of 19200
      88           0 :     uart->begin(19200);
      89             : 
      90           0 :     last_frame_us = AP_HAL::micros();
      91             : }
      92             : 
      93             : // read latest values from sensor and fill in x,y and totals.
      94           0 : void AP_OpticalFlow_CXOF::update(void)
      95             : {
      96             :     // sanity check uart
      97           0 :     if (uart == nullptr) {
      98           0 :         return;
      99             :     }
     100             : 
     101             :     // record gyro values as long as they are being used
     102             :     // the sanity check of dt below ensures old gyro values are not used
     103           0 :     if (gyro_sum_count < 1000) {
     104           0 :         const Vector3f& gyro = AP::ahrs().get_gyro();
     105           0 :         gyro_sum.x += gyro.x;
     106           0 :         gyro_sum.y += gyro.y;
     107           0 :         gyro_sum_count++;
     108             :     }
     109             : 
     110             :     // sensor values
     111           0 :     int32_t x_sum = 0;
     112           0 :     int32_t y_sum = 0;
     113           0 :     uint16_t qual_sum = 0;
     114           0 :     uint16_t count = 0;
     115             : 
     116             :     // read any available characters in the serial buffer
     117           0 :     int16_t nbytes = uart->available();
     118           0 :     while (nbytes-- > 0) {
     119             :         uint8_t c;
     120           0 :         if (!uart->read(c)) {
     121           0 :             continue;
     122             :         }
     123             :         // if buffer is empty and this byte is header, add to buffer
     124           0 :         if (buf_len == 0) {
     125           0 :             if (c == CXOF_HEADER) {
     126           0 :                 buf[buf_len++] = c;
     127             :             }
     128             :         } else {
     129             :             // add character to buffer
     130           0 :             buf[buf_len++] = c;
     131             : 
     132             :             // if buffer has 9 items try to decode it
     133           0 :             if (buf_len >= CXOF_FRAME_LENGTH) {
     134             :                 // check last character matches footer
     135           0 :                 if (buf[buf_len-1] != CXOF_FOOTER) {
     136           0 :                     buf_len = 0;
     137           0 :                     continue;
     138             :                 }
     139             : 
     140             :                 // decode package
     141           0 :                 int16_t x_raw = (int16_t)((uint16_t)buf[3] << 8) | buf[2];
     142           0 :                 int16_t y_raw = (int16_t)((uint16_t)buf[5] << 8) | buf[4];
     143             : 
     144             :                 // add to sum of all readings from sensor this iteration
     145           0 :                 count++;
     146           0 :                 x_sum += x_raw;
     147           0 :                 y_sum += y_raw;
     148           0 :                 qual_sum += buf[7];
     149             : 
     150             :                 // clear buffer
     151           0 :                 buf_len = 0;
     152             :             }
     153             :         }
     154             :     }
     155             : 
     156             :     // return without updating state if no readings
     157           0 :     if (count == 0) {
     158           0 :         return;
     159             :     }
     160             : 
     161           0 :     struct AP_OpticalFlow::OpticalFlow_state state {};
     162             : 
     163             :     // average surface quality scaled to be between 0 and 255
     164           0 :     state.surface_quality = (constrain_int16(qual_sum / count, 64, 78) - 64) * 255 / 14;
     165             : 
     166             :     // calculate dt
     167           0 :     uint64_t this_frame_us = uart->receive_time_constraint_us(CXOF_FRAME_LENGTH);
     168           0 :     if (this_frame_us == 0) {
     169             :         // for HAL that cannot estimate arrival time in serial buffer use current time
     170           0 :         this_frame_us = AP_HAL::micros();
     171             :     }
     172           0 :     float dt = (this_frame_us - last_frame_us) * 1.0e-6;
     173           0 :     last_frame_us = this_frame_us;
     174             : 
     175             :     // sanity check dt
     176           0 :     if (is_positive(dt) && (dt < CXOF_TIMEOUT_SEC)) {
     177             :         // calculate flow values
     178           0 :         const Vector2f flowScaler = _flowScaler();
     179           0 :         float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
     180           0 :         float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
     181             : 
     182             :         // copy flow rates to state structure
     183           0 :         state.flowRate = Vector2f(((float)x_sum / count) * flowScaleFactorX,
     184           0 :                                   ((float)y_sum / count) * flowScaleFactorY);
     185           0 :         state.flowRate *= CXOF_PIXEL_SCALING / dt;
     186             : 
     187             :         // copy average body rate to state structure
     188           0 :         state.bodyRate = Vector2f(gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count);
     189             : 
     190             :         // we only apply yaw to flowRate as body rate comes from AHRS
     191           0 :         _applyYaw(state.flowRate);
     192             :     } else {
     193             :         // first frame received in some time so cannot calculate flow values
     194           0 :         state.flowRate.zero();
     195           0 :         state.bodyRate.zero();
     196             :     }
     197             : 
     198           0 :     _update_frontend(state);
     199             : 
     200             :     // reset gyro sum
     201           0 :     gyro_sum.zero();
     202           0 :     gyro_sum_count = 0;
     203             : }
     204             : 
     205             : #endif  // AP_OPTICALFLOW_CXOF_ENABLED