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             : #include "AP_OpticalFlow_MAV.h"
      17             : 
      18             : #if AP_OPTICALFLOW_MAV_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <AP_AHRS/AP_AHRS.h>
      22             : 
      23             : #define OPTFLOW_MAV_TIMEOUT_SEC 0.5f
      24             : 
      25             : // detect the device
      26           0 : AP_OpticalFlow_MAV *AP_OpticalFlow_MAV::detect(AP_OpticalFlow &_frontend)
      27             : {
      28             :     // we assume mavlink messages will be sent into this driver
      29           0 :     AP_OpticalFlow_MAV *sensor = NEW_NOTHROW AP_OpticalFlow_MAV(_frontend);
      30           0 :     return sensor;
      31             : }
      32             : 
      33             : // read latest values from sensor and fill in x,y and totals.
      34           0 : void AP_OpticalFlow_MAV::update(void)
      35             : {
      36             :     // record gyro values as long as they are being used
      37             :     // the sanity check of dt below ensures old gyro values are not used
      38           0 :     if (gyro_sum_count < 1000) {
      39           0 :         const Vector3f& gyro = AP::ahrs().get_gyro();
      40           0 :         gyro_sum.x += gyro.x;
      41           0 :         gyro_sum.y += gyro.y;
      42           0 :         gyro_sum_count++;
      43             :     }
      44             : 
      45             :     // return without updating state if no readings
      46           0 :     if (count == 0) {
      47           0 :         return;
      48             :     }
      49             : 
      50           0 :     struct AP_OpticalFlow::OpticalFlow_state state {};
      51             : 
      52           0 :     state.surface_quality = quality_sum / count;
      53             : 
      54             :     // calculate dt
      55           0 :     const float dt = (latest_frame_us - prev_frame_us) * 1.0e-6;
      56           0 :     prev_frame_us = latest_frame_us;
      57             : 
      58             :     // sanity check dt
      59           0 :     if (is_positive(dt) && (dt < OPTFLOW_MAV_TIMEOUT_SEC)) {
      60             :         // calculate flow values
      61           0 :         const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x;
      62           0 :         const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y;
      63             : 
      64             :         // scale and copy flow rates to state structure
      65             :         // if using flow_rates these are in rad/s (as opposed to pixels) and do not need to be multiplied by dt
      66           0 :         const float dt_used = flow_sum_is_rads ? 1.0f : dt;
      67           0 :         state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt_used,
      68           0 :                            ((float)flow_sum.y / count) * flow_scale_factor_y * dt_used };
      69             : 
      70             :         // copy body-rates from gyro or set to zero
      71           0 :         if (option_is_enabled(Option::Stabilised)) {
      72             :             // if the sensor is stabilised then body rates are always zero
      73           0 :             state.bodyRate.zero();
      74             :         } else {
      75             :             // copy average body rate to state structure
      76           0 :             state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count };   
      77             :         }
      78             : 
      79             :         // we only apply yaw to flowRate as body rate comes from AHRS
      80           0 :         _applyYaw(state.flowRate);
      81             :     } else {
      82             :         // first frame received in some time so cannot calculate flow values
      83           0 :         state.flowRate.zero();
      84           0 :         state.bodyRate.zero();
      85             :     }
      86             : 
      87           0 :     _update_frontend(state);
      88             : 
      89             :     // reset local buffers
      90           0 :     flow_sum.zero();
      91           0 :     quality_sum = 0;
      92           0 :     count = 0;
      93             : 
      94             :     // reset gyro sum
      95           0 :     gyro_sum.zero();
      96           0 :     gyro_sum_count = 0;
      97             : }
      98             : 
      99             : // handle OPTICAL_FLOW mavlink messages
     100           0 : void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg)
     101             : {
     102             :     mavlink_optical_flow_t packet;
     103           0 :     mavlink_msg_optical_flow_decode(&msg, &packet);
     104             : 
     105             :     // record time message was received
     106             :     // ToDo: add jitter correction
     107           0 :     latest_frame_us = AP_HAL::micros64();
     108             : 
     109             :     // use flow_rate_x/y fields if non-zero values are ever provided
     110           0 :     if (!flow_sum_is_rads && (!is_zero(packet.flow_rate_x) || !is_zero(packet.flow_rate_y))) {
     111           0 :         flow_sum_is_rads = true;
     112           0 :         flow_sum.zero();
     113           0 :         quality_sum = 0;
     114           0 :         count = 0;
     115             :     }
     116             : 
     117             :     // add sensor values to sum
     118           0 :     if (flow_sum_is_rads) {
     119             :         // higher precision flow_rate_x/y fields are used
     120           0 :         flow_sum.x += packet.flow_rate_x;
     121           0 :         flow_sum.y += packet.flow_rate_y;
     122             :     } else {
     123             :         // lower precision flow_x/y fields are used
     124           0 :         flow_sum.x += packet.flow_x;
     125           0 :         flow_sum.y += packet.flow_y;
     126             :     }
     127           0 :     quality_sum += packet.quality;
     128           0 :     count++;
     129             : 
     130             :     // take sensor id from message
     131           0 :     sensor_id = packet.sensor_id;
     132           0 : }
     133             : 
     134             : #endif  // AP_OPTICALFLOW_MAV_ENABLED