Line data    Source code

       1             : #include "AP_OpticalFlow_HereFlow.h"
       2             : 
       3             : #if AP_OPTICALFLOW_HEREFLOW_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : 
       7             : #include <AP_CANManager/AP_CANManager.h>
       8             : #include <AP_DroneCAN/AP_DroneCAN.h>
       9             : #include <AP_BoardConfig/AP_BoardConfig.h>
      10             : 
      11             : extern const AP_HAL::HAL& hal;
      12             : 
      13             : uint8_t AP_OpticalFlow_HereFlow::_node_id = 0;
      14             : AP_OpticalFlow_HereFlow* AP_OpticalFlow_HereFlow::_driver = nullptr;
      15             : AP_DroneCAN* AP_OpticalFlow_HereFlow::_ap_dronecan = nullptr;
      16             : /*
      17             :   constructor - registers instance at top Flow driver
      18             :  */
      19           0 : AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow) :
      20           0 :     OpticalFlow_backend(flow)
      21             : {
      22           0 :     if (_driver) {
      23           0 :         AP_HAL::panic("Only one instance of Flow supported!");
      24             :     }
      25           0 :     _driver = this;
      26           0 : }
      27             : 
      28             : //links the HereFlow messages to the backend
      29           5 : bool AP_OpticalFlow_HereFlow::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      30             : {
      31           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      32             : 
      33           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr);
      34             : }
      35             : 
      36             : //updates driver states based on received HereFlow messages
      37           0 : void AP_OpticalFlow_HereFlow::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg)
      38             : {
      39           0 :     if (_driver == nullptr) {
      40           0 :         return;
      41             :     }
      42             :     //protect from data coming from duplicate sensors,
      43             :     //as we only handle one Here Flow at a time as of now
      44           0 :     if (_ap_dronecan == nullptr) {
      45           0 :         _ap_dronecan = ap_dronecan;
      46           0 :         _node_id = transfer.source_node_id;
      47             :     }
      48             : 
      49           0 :     if (_ap_dronecan == ap_dronecan && _node_id == transfer.source_node_id) {
      50           0 :         WITH_SEMAPHORE(_driver->_sem);
      51           0 :         _driver->new_data = true;
      52           0 :         _driver->flow_integral = Vector2f(msg.flow_integral[0], msg.flow_integral[1]);
      53           0 :         _driver->rate_gyro_integral = Vector2f(msg.rate_gyro_integral[0], msg.rate_gyro_integral[1]);
      54           0 :         _driver->integral_time = msg.integration_interval;
      55           0 :         _driver->surface_quality = msg.quality;
      56           0 :     }
      57             : }
      58             : 
      59           0 : void AP_OpticalFlow_HereFlow::update()
      60             : {
      61           0 :     _push_state();
      62           0 : }
      63             : 
      64             : // Read the sensor
      65           0 : void AP_OpticalFlow_HereFlow::_push_state(void)
      66             : {
      67           0 :     WITH_SEMAPHORE(_sem);
      68           0 :     if (!new_data) {
      69           0 :         return;
      70             :     }
      71           0 :     struct AP_OpticalFlow::OpticalFlow_state state;
      72           0 :     const Vector2f flowScaler = _flowScaler();
      73             :     //setup scaling based on parameters
      74           0 :     float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
      75           0 :     float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
      76           0 :     float integralToRate = 1.0f / integral_time;
      77             :     //Convert to Raw Flow measurement to Flow Rate measurement
      78           0 :     state.flowRate = Vector2f{
      79           0 :         flow_integral.x * flowScaleFactorX,
      80           0 :         flow_integral.y * flowScaleFactorY
      81           0 :     } * integralToRate;
      82           0 :     state.bodyRate = rate_gyro_integral * integralToRate;
      83           0 :     state.surface_quality = surface_quality;
      84           0 :     _applyYaw(state.flowRate);
      85           0 :     _applyYaw(state.bodyRate);
      86             :     // hal.console->printf("DRV: %u %f %f\n", state.surface_quality, flowRate.length(), bodyRate.length());
      87           0 :     _update_frontend(state);
      88           0 :     new_data = false;
      89           0 : }
      90             : 
      91             : #endif // AP_OPTICALFLOW_HEREFLOW_ENABLED