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             :  * AP_OpticalFlow_SITL.cpp - SITL emulation of optical flow sensor.
      18             :  */
      19             : 
      20             : #include "AP_OpticalFlow_SITL.h"
      21             : 
      22             : #if AP_OPTICALFLOW_SITL_ENABLED
      23             : 
      24             : #include <AP_HAL/AP_HAL.h>
      25             : #include <SITL/SITL.h>
      26             : 
      27      202181 : void AP_OpticalFlow_SITL::update(void)
      28             : {
      29      202181 :     auto *_sitl = AP::sitl();
      30             : 
      31      202181 :     if (!_sitl->flow_enable) {
      32      192053 :         return;
      33             :     }
      34             : 
      35             :     // update at the requested rate
      36      202178 :     uint32_t now = AP_HAL::millis();
      37      202178 :     if (now - last_flow_ms < 1000*(1.0f/_sitl->flow_rate)) {
      38      192050 :         return;
      39             :     }
      40       10128 :     last_flow_ms = now;
      41             : 
      42       10128 :     Vector3f gyro(radians(_sitl->state.rollRate), 
      43       10128 :                   radians(_sitl->state.pitchRate), 
      44       10128 :                   radians(_sitl->state.yawRate));
      45             : 
      46       10128 :     AP_OpticalFlow::OpticalFlow_state state;
      47             : 
      48             :     // NED velocity vector in m/s
      49       10128 :     Vector3f velocity(_sitl->state.speedN,
      50       10128 :                       _sitl->state.speedE,
      51       10128 :                       _sitl->state.speedD);
      52             : 
      53             :     // a rotation matrix following DCM conventions
      54       10128 :     Matrix3f rotmat;
      55       10128 :     rotmat.from_euler(radians(_sitl->state.rollDeg),
      56       10128 :                       radians(_sitl->state.pitchDeg),
      57       10128 :                       radians(_sitl->state.yawDeg));
      58             : 
      59             : 
      60       10128 :     state.surface_quality = 51;
      61             : 
      62             :     // sensor position offset in body frame
      63       10128 :     Vector3f posRelSensorBF = _sitl->optflow_pos_offset;
      64             : 
      65             :     // estimate range to centre of image
      66             :     float range;
      67       10128 :     if (rotmat.c.z > 0.05f && _sitl->state.height_agl > 0) {
      68        9493 :         Vector3f relPosSensorEF = rotmat * posRelSensorBF;
      69        9493 :         range = (_sitl->state.height_agl - relPosSensorEF.z) / rotmat.c.z;
      70        9493 :     } else {
      71         635 :         range = 1e38f;
      72             :     }
      73             : 
      74             :     // Calculate relative velocity in sensor frame assuming no misalignment between sensor and vehicle body axes
      75       10128 :     Vector3f relVelSensor = rotmat.mul_transpose(velocity);
      76             : 
      77             :     // correct relative velocity for rotation rates and sensor offset
      78       10128 :     relVelSensor += gyro % posRelSensorBF;
      79             : 
      80             :     // scaling based on parameters
      81       10128 :     const Vector2f flowScaler = _flowScaler();
      82       10128 :     const float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
      83       10128 :     const float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
      84             : 
      85             :     // Divide velocity by range and add body rates to get predicted sensed angular
      86             :     // optical rates relative to X and Y sensor axes assuming no misalignment or scale
      87             :     // factor error. Note - these are instantaneous values. The sensor sums these values across the interval from the last
      88             :     // poll to provide a delta angle across the interface
      89       10128 :     state.flowRate.x = (-relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float()) * flowScaleFactorX;
      90       10128 :     state.flowRate.y =  (relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float()) * flowScaleFactorY;
      91             : 
      92             :     // The flow sensors body rates are assumed to be the same as the vehicle body rates (ie no misalignment)
      93             :     // Note - these are instantaneous values. The sensor sums these values across the interval from the last
      94             :     // poll to provide a delta angle across the interface.
      95       10128 :     state.bodyRate = Vector2f(gyro.x, gyro.y);
      96             : 
      97       10128 :     optflow_data[next_optflow_index++] = state;
      98       10128 :     if (next_optflow_index >= optflow_delay+1) {
      99       10128 :         next_optflow_index = 0;
     100             :     }
     101             : 
     102       10128 :     state = optflow_data[next_optflow_index];
     103             : 
     104       10128 :     if (_sitl->flow_delay != optflow_delay) {
     105             :         // cope with updates to the delay control
     106           0 :         if (_sitl->flow_delay > 0 &&
     107           0 :             (uint8_t)(_sitl->flow_delay) > ARRAY_SIZE(optflow_data)) {
     108           0 :             _sitl->flow_delay.set(ARRAY_SIZE(optflow_data));
     109             :         }
     110           0 :         optflow_delay = _sitl->flow_delay;
     111           0 :         for (uint8_t i=0; i<optflow_delay; i++) {
     112           0 :             optflow_data[i] = state;
     113             :         }
     114             :     }
     115             : 
     116       10128 :     _applyYaw(state.flowRate);
     117             :     
     118             :     // copy results to front end
     119       10128 :     _update_frontend(state);
     120             : }
     121             : 
     122             : #endif  // AP_OPTICALFLOW_SITL_ENABLED