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 : #pragma once
16 :
17 : #include "AP_VisualOdom_config.h"
18 :
19 : #if HAL_VISUALODOM_ENABLED
20 :
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_Param/AP_Param.h>
23 : #include <GCS_MAVLink/GCS_config.h>
24 : #if HAL_GCS_ENABLED
25 : #include <GCS_MAVLink/GCS_MAVLink.h>
26 : #endif
27 : #include <AP_Math/AP_Math.h>
28 :
29 : class AP_VisualOdom_Backend;
30 :
31 : #define AP_VISUALODOM_TIMEOUT_MS 300
32 :
33 : class AP_VisualOdom
34 : {
35 : public:
36 :
37 : AP_VisualOdom();
38 :
39 : // get singleton instance
40 1838123 : static AP_VisualOdom *get_singleton() {
41 1838123 : return _singleton;
42 : }
43 :
44 : // external position backend types (used by _TYPE parameter)
45 : enum class VisualOdom_Type {
46 : None = 0,
47 : #if AP_VISUALODOM_MAV_ENABLED
48 : MAV = 1,
49 : #endif
50 : #if AP_VISUALODOM_INTELT265_ENABLED
51 : IntelT265 = 2,
52 : VOXL = 3,
53 : #endif
54 : };
55 :
56 : // detect and initialise any sensors
57 : void init();
58 :
59 : // return true if sensor is enabled
60 : bool enabled() const;
61 :
62 : // return true if sensor is basically healthy (we are receiving data)
63 : bool healthy() const;
64 :
65 : // get user defined orientation
66 3636 : enum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }
67 :
68 : // get user defined scaling applied to position estimates
69 10069 : float get_pos_scale() const { return _pos_scale; }
70 :
71 : // return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
72 288426 : const Vector3f &get_pos_offset(void) const { return _pos_offset; }
73 :
74 : // return the sensor delay in milliseconds (see _DELAY_MS parameter)
75 302773 : uint16_t get_delay_ms() const { return MAX(0, _delay_ms); }
76 :
77 : // return velocity measurement noise in m/s
78 7164 : float get_vel_noise() const { return _vel_noise; }
79 :
80 : // return position measurement noise in m
81 10069 : float get_pos_noise() const { return _pos_noise; }
82 :
83 : // return yaw measurement noise in rad
84 10069 : float get_yaw_noise() const { return _yaw_noise; }
85 :
86 : // return quality threshold
87 14347 : int8_t get_quality_min() const { return _quality_min; }
88 :
89 : // return quality as a measure from -1 ~ 100
90 : // -1 means failed, 0 means unknown, 1 is worst, 100 is best
91 : int8_t quality() const;
92 :
93 : #if HAL_GCS_ENABLED
94 : // consume vision_position_delta mavlink messages
95 : void handle_vision_position_delta_msg(const mavlink_message_t &msg);
96 : #endif
97 :
98 : // general purpose methods to consume position estimate data and send to EKF
99 : // distances in meters, roll, pitch and yaw are in radians
100 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
101 : void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
102 : 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);
103 :
104 : // general purpose methods to consume velocity estimate data and send to EKF
105 : // velocity in NED meters per second
106 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
107 : // vel_err is the velocity error (m/s), 0 means unknown
108 : 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);
109 :
110 : // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
111 : void request_align_yaw_to_ahrs();
112 :
113 : // update position offsets to align to AHRS position
114 : // should only be called when this library is not being used as the position source
115 : void align_position_to_ahrs(bool align_xy, bool align_z);
116 :
117 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
118 : bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
119 :
120 : static const struct AP_Param::GroupInfo var_info[];
121 :
122 22998 : VisualOdom_Type get_type(void) const {
123 22998 : return _type;
124 : }
125 :
126 : private:
127 :
128 : static AP_VisualOdom *_singleton;
129 :
130 : // parameters
131 : AP_Enum<VisualOdom_Type> _type; // sensor type
132 : AP_Vector3f _pos_offset; // position offset of the camera in the body frame
133 : AP_Int8 _orientation; // camera orientation on vehicle frame
134 : AP_Float _pos_scale; // position scale factor applied to sensor values
135 : AP_Int16 _delay_ms; // average delay relative to inertial measurements
136 : AP_Float _vel_noise; // velocity measurement noise in m/s
137 : AP_Float _pos_noise; // position measurement noise in meters
138 : AP_Float _yaw_noise; // yaw measurement noise in radians
139 : AP_Int8 _quality_min; // positions and velocities will only be sent to EKF if over this value. if 0 all values sent to EKF
140 :
141 : // reference to backends
142 : AP_VisualOdom_Backend *_driver;
143 : };
144 :
145 : namespace AP {
146 : AP_VisualOdom *visualodom();
147 : };
148 :
149 : #endif // HAL_VISUALODOM_ENABLED
|
