libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h

   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, uint8_t reset_counter, int8_t quality) override;
  23
  24     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
  25     void request_align_yaw_to_ahrs() override { _align_yaw = true; }
  26
  27     // update position offsets to align to AHRS position
  28     // should only be called when this library is not being used as the position source
  29     void align_position_to_ahrs(bool align_xy, bool align_z) override { _align_posxy = align_xy; _align_posz = align_z; }
  30
  31     // arming check
  32     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
  33
  34 protected:
  35
  36     // apply rotation and correction to position
  37     void rotate_and_correct_position(Vector3f &position) const;
  38
  39     // apply rotation to velocity
  40     void rotate_velocity(Vector3f &velocity) const;
  41
  42     // rotate attitude using _yaw_trim
  43     void rotate_attitude(Quaternion &attitude) const;
  44
  45     // use sensor provided position and attitude to calculate rotation to align sensor yaw with AHRS/EKF attitude
  46     // calls align_yaw (see below)
  47     bool align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude);
  48
  49     // align sensor yaw with any new yaw (in radians)
  50     void align_yaw(const Vector3f &position, const Quaternion &attitude, float yaw_rad);
  51
  52     // returns true if sensor data should be consumed, false if it should be ignored
  53     // set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise
  54     // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored
  55     bool should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter);
  56
  57     // align position with ahrs position by updating _pos_correction
  58     // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
  59     bool align_position_to_ahrs(const Vector3f &sensor_pos, bool align_xy, bool align_z);
  60
  61     // align position with a new position by updating _pos_correction
  62     // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
  63     // new_pos should be a NED position offset from the EKF origin
  64     void align_position(const Vector3f &sensor_pos, const Vector3f &new_pos, bool align_xy, bool align_z);
  65
  66     // record voxl camera's position and reset counter for reset jump handling
  67     // position is post scaling, offset and orientation corrections
  68     void record_voxl_position_and_reset_count(const Vector3f &position, uint8_t reset_counter);
  69
  70     // handle voxl camera reset jumps in attitude and position
  71     // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
  72     // sensor_att is similarly the attitude directly from the sensor
  73     void handle_voxl_camera_reset_jump(const Vector3f &sensor_pos, const Quaternion &sensor_att, uint8_t reset_counter);
  74
  75     float _yaw_trim;                            // yaw angle trim (in radians) to align camera's yaw to ahrs/EKF's
  76     Quaternion _yaw_rotation;                   // earth-frame yaw rotation to align heading of sensor with vehicle.  use when _yaw_trim is non-zero
  77     Quaternion _att_rotation;                   // body-frame rotation corresponding to ORIENT parameter.  use when get_orientation != NONE
  78     Matrix3f _posvel_rotation;                  // rotation to align position and/or velocity from sensor to earth frame.  use when _use_posvel_rotation is true
  79     Vector3f _pos_correction;                   // position correction that should be added to position reported from sensor
  80     bool _use_att_rotation;                     // true if _att_rotation should be applied to sensor's attitude data
  81     bool _use_posvel_rotation;                  // true if _posvel_rotation should be applied to sensor's position and/or velocity data
  82     bool _align_yaw = true;                     // true if sensor yaw should be aligned to AHRS/EKF
  83     bool _align_posxy;                          // true if sensor xy position should be aligned to AHRS
  84     bool _align_posz;                           // true if sensor z position should be aligned to AHRS
  85     bool _error_orientation;                    // true if the orientation is not supported
  86     Quaternion _attitude_last;                  // last attitude received from camera (used for arming checks)
  87     uint8_t _pos_reset_counter_last;            // last vision-position-estimate reset counter value
  88     uint32_t _pos_reset_ignore_start_ms;        // system time we start ignoring sensor information, 0 if sensor data is not being ignored
  89
  90     // voxl reset jump handling variables
  91     uint8_t _voxl_reset_counter_last;           // last reset counter from voxl camera (only used for origin jump handling)
  92     Vector3f _voxl_position_last;               // last recorded position (post scaling, offset and orientation corrections)
  93 };
  94
  95 #endif  // AP_VISUALODOM_INTELT265_ENABLED