Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_VisualOdom_config.h"
       4             : 
       5             : #if AP_VISUALODOM_INTELT265_ENABLED
       6             : 
       7             : #include "AP_VisualOdom_Backend.h"
       8             : 
       9             : class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
      10             : {
      11             : 
      12             : public:
      13             : 
      14             :     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
      15             : 
      16             :     // consume vision pose estimate data and send to EKF. distances in meters
      17             :     // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
      18             :     void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) override;
      19             : 
      20             :     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
      21             :     // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
      22             :     void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality) override;
      23             : 
      24             :     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
      25           1 :     void request_align_yaw_to_ahrs() override { _align_yaw = true; }
      26             : 
      27             :     // arming check
      28             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
      29             : 
      30             : protected:
      31             : 
      32             :     // apply rotation to velocity
      33             :     void rotate_velocity(Vector3f &velocity) const;
      34             : 
      35             :     // rotate attitude using _yaw_trim
      36             :     void rotate_attitude(Quaternion &attitude) const;
      37             : 
      38             :     // use sensor provided position and attitude to calculate rotation to align sensor yaw with AHRS/EKF attitude
      39             :     // calls align_yaw (see below)
      40             :     bool align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude);
      41             : 
      42             :     // align sensor yaw with any new yaw (in radians)
      43             :     void align_yaw(const Vector3f &position, const Quaternion &attitude, float yaw_rad);
      44             : 
      45             :     // returns true if sensor data should be consumed, false if it should be ignored
      46             :     // set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise
      47             :     // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored
      48             :     bool should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter);
      49             : 
      50             :     // record voxl camera's position and reset counter for reset jump handling
      51             :     // position is post scaling, offset and orientation corrections
      52             :     void record_voxl_position_and_reset_count(const Vector3f &position, uint8_t reset_counter);
      53             : 
      54             :     // handle voxl camera reset jumps in attitude and position
      55             :     // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
      56             :     // sensor_att is similarly the attitude directly from the sensor
      57             :     void handle_voxl_camera_reset_jump(const Vector3f &sensor_pos, const Quaternion &sensor_att, uint8_t reset_counter);
      58             : 
      59             :     float _yaw_trim;                            // yaw angle trim (in radians) to align camera's yaw to ahrs/EKF's
      60             :     Quaternion _yaw_rotation;                   // earth-frame yaw rotation to align heading of sensor with vehicle.  use when _yaw_trim is non-zero
      61             :     Quaternion _att_rotation;                   // body-frame rotation corresponding to ORIENT parameter.  use when get_orientation != NONE
      62             :     bool _use_att_rotation;                     // true if _att_rotation should be applied to sensor's attitude data
      63             :     bool _align_yaw = true;                     // true if sensor yaw should be aligned to AHRS/EKF
      64             :     bool _error_orientation;                    // true if the orientation is not supported
      65             :     Quaternion _attitude_last;                  // last attitude received from camera (used for arming checks)
      66             :     uint8_t _pos_reset_counter_last;            // last vision-position-estimate reset counter value
      67             :     uint32_t _pos_reset_ignore_start_ms;        // system time we start ignoring sensor information, 0 if sensor data is not being ignored
      68             : 
      69             :     // voxl reset jump handling variables
      70             :     uint8_t _voxl_reset_counter_last;           // last reset counter from voxl camera (only used for origin jump handling)
      71             :     Vector3f _voxl_position_last;               // last recorded position (post scaling, offset and orientation corrections)
      72             : };
      73             : 
      74             : #endif  // AP_VISUALODOM_INTELT265_ENABLED