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 : #include "AP_VisualOdom_config.h"
17 :
18 : #if HAL_VISUALODOM_ENABLED
19 :
20 : #include "AP_VisualOdom.h"
21 : #include "AP_VisualOdom_Backend.h"
22 : #include "AP_VisualOdom_MAV.h"
23 : #include "AP_VisualOdom_IntelT265.h"
24 : #include <AP_AHRS/AP_AHRS.h>
25 :
26 : extern const AP_HAL::HAL &hal;
27 :
28 : // table of user settable parameters
29 : const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
30 :
31 : // @Param: _TYPE
32 : // @DisplayName: Visual odometry camera connection type
33 : // @Description: Visual odometry camera connection type
34 : // @Values: 0:None,1:MAVLink,2:IntelT265,3:VOXL(ModalAI)
35 : // @User: Advanced
36 : // @RebootRequired: True
37 : AP_GROUPINFO_FLAGS("_TYPE", 0, AP_VisualOdom, _type, 0, AP_PARAM_FLAG_ENABLE),
38 :
39 : // @Param: _POS_X
40 : // @DisplayName: Visual odometry camera X position offset
41 : // @Description: X position of the camera in body frame. Positive X is forward of the origin.
42 : // @Units: m
43 : // @Range: -5 5
44 : // @Increment: 0.01
45 : // @User: Advanced
46 :
47 : // @Param: _POS_Y
48 : // @DisplayName: Visual odometry camera Y position offset
49 : // @Description: Y position of the camera in body frame. Positive Y is to the right of the origin.
50 : // @Units: m
51 : // @Range: -5 5
52 : // @Increment: 0.01
53 : // @User: Advanced
54 :
55 : // @Param: _POS_Z
56 : // @DisplayName: Visual odometry camera Z position offset
57 : // @Description: Z position of the camera in body frame. Positive Z is down from the origin.
58 : // @Units: m
59 : // @Range: -5 5
60 : // @Increment: 0.01
61 : // @User: Advanced
62 : AP_GROUPINFO("_POS", 1, AP_VisualOdom, _pos_offset, 0.0f),
63 :
64 : // @Param: _ORIENT
65 : // @DisplayName: Visual odometery camera orientation
66 : // @Description: Visual odometery camera orientation
67 : // @Values: 0:Forward, 2:Right, 4:Back, 6:Left, 24:Up, 25:Down
68 : // @User: Advanced
69 : AP_GROUPINFO("_ORIENT", 2, AP_VisualOdom, _orientation, ROTATION_NONE),
70 :
71 : // @Param: _SCALE
72 : // @DisplayName: Visual odometry scaling factor
73 : // @Description: Visual odometry scaling factor applied to position estimates from sensor
74 : // @User: Advanced
75 : AP_GROUPINFO("_SCALE", 3, AP_VisualOdom, _pos_scale, 1.0f),
76 :
77 : // @Param: _DELAY_MS
78 : // @DisplayName: Visual odometry sensor delay
79 : // @Description: Visual odometry sensor delay relative to inertial measurements
80 : // @Units: ms
81 : // @Range: 0 250
82 : // @User: Advanced
83 : AP_GROUPINFO("_DELAY_MS", 4, AP_VisualOdom, _delay_ms, 10),
84 :
85 : // @Param: _VEL_M_NSE
86 : // @DisplayName: Visual odometry velocity measurement noise
87 : // @Description: Visual odometry velocity measurement noise in m/s
88 : // @Units: m/s
89 : // @Range: 0.05 5.0
90 : // @User: Advanced
91 : AP_GROUPINFO("_VEL_M_NSE", 5, AP_VisualOdom, _vel_noise, 0.1),
92 :
93 : // @Param: _POS_M_NSE
94 : // @DisplayName: Visual odometry position measurement noise
95 : // @Description: Visual odometry position measurement noise minimum (meters). This value will be used if the sensor provides a lower noise value (or no noise value)
96 : // @Units: m
97 : // @Range: 0.1 10.0
98 : // @User: Advanced
99 : AP_GROUPINFO("_POS_M_NSE", 6, AP_VisualOdom, _pos_noise, 0.2f),
100 :
101 : // @Param: _YAW_M_NSE
102 : // @DisplayName: Visual odometry yaw measurement noise
103 : // @Description: Visual odometry yaw measurement noise minimum (radians), This value will be used if the sensor provides a lower noise value (or no noise value)
104 : // @Units: rad
105 : // @Range: 0.05 1.0
106 : // @User: Advanced
107 : AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),
108 :
109 : // @Param: _QUAL_MIN
110 : // @DisplayName: Visual odometry minimum quality
111 : // @Description: Visual odometry will only be sent to EKF if over this value. -1 to always send (even bad values), 0 to send if good or unknown
112 : // @Units: %
113 : // @Range: -1 100
114 : // @User: Advanced
115 : AP_GROUPINFO("_QUAL_MIN", 8, AP_VisualOdom, _quality_min, 0),
116 :
117 : AP_GROUPEND
118 : };
119 :
120 2098 : AP_VisualOdom::AP_VisualOdom()
121 : {
122 2098 : AP_Param::setup_object_defaults(this, var_info);
123 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
124 2096 : if (_singleton != nullptr) {
125 0 : AP_HAL::panic("AP_VisualOdom must be singleton");
126 : }
127 : #endif
128 2098 : _singleton = this;
129 2098 : }
130 :
131 : // detect and initialise any sensors
132 2086 : void AP_VisualOdom::init()
133 : {
134 : // create backend
135 2086 : switch (VisualOdom_Type(_type.get())) {
136 2078 : case VisualOdom_Type::None:
137 : // do nothing
138 2078 : break;
139 : #if AP_VISUALODOM_MAV_ENABLED
140 4 : case VisualOdom_Type::MAV:
141 4 : _driver = NEW_NOTHROW AP_VisualOdom_MAV(*this);
142 4 : break;
143 : #endif
144 : #if AP_VISUALODOM_INTELT265_ENABLED
145 4 : case VisualOdom_Type::IntelT265:
146 : case VisualOdom_Type::VOXL:
147 4 : _driver = NEW_NOTHROW AP_VisualOdom_IntelT265(*this);
148 4 : break;
149 : #endif
150 : }
151 2086 : }
152 :
153 : // return true if sensor is enabled
154 4421017 : bool AP_VisualOdom::enabled() const
155 : {
156 4421017 : return ((_type != VisualOdom_Type::None));
157 : }
158 :
159 : // return true if sensor is basically healthy (we are receiving data)
160 289405 : bool AP_VisualOdom::healthy() const
161 : {
162 289405 : if (!enabled()) {
163 2670 : return false;
164 : }
165 :
166 286735 : if (_driver == nullptr) {
167 518 : return false;
168 : }
169 286217 : return _driver->healthy();
170 : }
171 :
172 : // return quality as a measure from 0 ~ 100
173 : // -1 means failed, 0 means unknown, 1 is worst, 100 is best
174 0 : int8_t AP_VisualOdom::quality() const
175 : {
176 0 : if (_driver == nullptr) {
177 0 : return 0;
178 : }
179 0 : return _driver->quality();
180 : }
181 :
182 : #if HAL_GCS_ENABLED
183 : // consume vision_position_delta mavlink messages
184 3735 : void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &msg)
185 : {
186 : // exit immediately if not enabled
187 3735 : if (!enabled()) {
188 3 : return;
189 : }
190 :
191 : // call backend
192 3732 : if (_driver != nullptr) {
193 3633 : _driver->handle_vision_position_delta_msg(msg);
194 : }
195 : }
196 : #endif
197 :
198 : // general purpose method to consume position estimate data and send to EKF
199 : // distances in meters, roll, pitch and yaw are in radians
200 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
201 10472 : void AP_VisualOdom::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)
202 : {
203 : // exit immediately if not enabled
204 10472 : if (!enabled()) {
205 128 : return;
206 : }
207 :
208 : // call backend
209 10344 : if (_driver != nullptr) {
210 : // convert attitude to quaternion and call backend
211 10069 : Quaternion attitude;
212 10069 : attitude.from_euler(roll, pitch, yaw);
213 10069 : _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
214 : }
215 : }
216 :
217 : // general purpose method to consume position estimate data and send to EKF
218 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
219 0 : void AP_VisualOdom::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)
220 : {
221 : // exit immediately if not enabled
222 0 : if (!enabled()) {
223 0 : return;
224 : }
225 :
226 : // call backend
227 0 : if (_driver != nullptr) {
228 0 : _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
229 : }
230 : }
231 :
232 : // general purpose methods to consume velocity estimate data and send to EKF
233 : // velocity in NED meters per second
234 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
235 4438 : void AP_VisualOdom::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)
236 : {
237 : // exit immediately if not enabled
238 4438 : if (!enabled()) {
239 60 : return;
240 : }
241 :
242 : // call backend
243 4378 : if (_driver != nullptr) {
244 4278 : _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, vel_err, reset_counter, quality);
245 : }
246 : }
247 :
248 : // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
249 1 : void AP_VisualOdom::request_align_yaw_to_ahrs()
250 : {
251 : // exit immediately if not enabled
252 1 : if (!enabled()) {
253 0 : return;
254 : }
255 :
256 : // call backend
257 1 : if (_driver != nullptr) {
258 1 : _driver->request_align_yaw_to_ahrs();
259 : }
260 : }
261 :
262 : // update position offsets to align to AHRS position. Should only be called when this library is not being used as the position source
263 278478 : void AP_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z)
264 : {
265 : // exit immediately if not enabled
266 278478 : if (!enabled()) {
267 0 : return;
268 : }
269 :
270 : // call backend
271 278478 : if (_driver != nullptr) {
272 278478 : _driver->align_position_to_ahrs(align_xy, align_z);
273 : }
274 : }
275 :
276 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
277 133080 : bool AP_VisualOdom::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
278 : {
279 : // exit immediately if not enabled
280 133080 : if (!enabled()) {
281 132925 : return true;
282 : }
283 :
284 : // if no backend we must have failed to create because out of memory
285 155 : if (_driver == nullptr) {
286 4 : hal.util->snprintf(failure_msg, failure_msg_len, "out of memory");
287 4 : return false;
288 : }
289 :
290 : // check healthy
291 151 : if (!healthy()) {
292 0 : hal.util->snprintf(failure_msg, failure_msg_len, "not healthy");
293 0 : return false;
294 : }
295 :
296 : // call backend specific arming check
297 151 : return _driver->pre_arm_check(failure_msg, failure_msg_len);
298 : }
299 :
300 : // singleton instance
301 : AP_VisualOdom *AP_VisualOdom::_singleton;
302 :
303 : namespace AP {
304 :
305 1838123 : AP_VisualOdom *visualodom()
306 : {
307 1838123 : return AP_VisualOdom::get_singleton();
308 : }
309 :
310 : }
311 :
312 : #endif
|
