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 AP_VISUALODOM_INTELT265_ENABLED
19 :
20 : #include "AP_VisualOdom_IntelT265.h"
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <AP_AHRS/AP_AHRS.h>
24 : #include <GCS_MAVLink/GCS.h>
25 :
26 : #define VISUALODOM_RESET_IGNORE_DURATION_MS 1000 // sensor data is ignored for 1sec after a position reset
27 :
28 : extern const AP_HAL::HAL& hal;
29 :
30 : // consume vision pose estimate data and send to EKF. distances in meters
31 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
32 6704 : void AP_VisualOdom_IntelT265::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)
33 : {
34 6704 : const float scale_factor = _frontend.get_pos_scale();
35 6704 : Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
36 6704 : Quaternion att = attitude;
37 :
38 : // handle voxl camera reset jumps in attitude and position
39 6704 : handle_voxl_camera_reset_jump(pos, att, reset_counter);
40 :
41 : // handle request to align sensor's yaw with vehicle's AHRS/EKF attitude
42 6704 : if (_align_yaw) {
43 1186 : if (align_yaw_to_ahrs(pos, attitude)) {
44 3 : _align_yaw = false;
45 : }
46 : }
47 6704 : if (_align_posxy || _align_posz) {
48 0 : if (align_position_to_ahrs(pos, _align_posxy, _align_posz)) {
49 0 : _align_posxy = _align_posz = false;
50 : }
51 : }
52 :
53 : // rotate position and attitude to align with vehicle
54 6704 : rotate_and_correct_position(pos);
55 6704 : rotate_attitude(att);
56 :
57 : // record position for voxl reset jump handling
58 6704 : record_voxl_position_and_reset_count(pos, reset_counter);
59 :
60 6704 : posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
61 6704 : angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
62 :
63 : // record quality
64 6704 : _quality = quality;
65 :
66 : // check for recent position reset
67 6704 : bool consume = should_consume_sensor_data(true, reset_counter) && (_quality >= _frontend.get_quality_min());
68 6704 : if (consume) {
69 : // send attitude and position to EKF
70 6704 : AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
71 : }
72 :
73 : // calculate euler orientation for logging
74 : float roll;
75 : float pitch;
76 : float yaw;
77 6704 : att.to_euler(roll, pitch, yaw);
78 :
79 : #if HAL_LOGGING_ENABLED
80 : // log sensor data
81 6704 : Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume, _quality);
82 : #endif
83 :
84 : // store corrected attitude for use in pre-arm checks
85 6704 : _attitude_last = att;
86 :
87 : // record time for health monitoring
88 6704 : _last_update_ms = AP_HAL::millis();
89 6704 : }
90 :
91 : // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
92 : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
93 2886 : void AP_VisualOdom_IntelT265::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)
94 : {
95 : // rotate velocity to align with vehicle
96 2886 : Vector3f vel_corrected = vel;
97 2886 : rotate_velocity(vel_corrected);
98 :
99 : // record quality
100 2886 : _quality = quality;
101 :
102 : // check for recent position reset
103 2886 : bool consume = should_consume_sensor_data(false, reset_counter) && (_quality >= _frontend.get_quality_min());
104 2886 : if (consume) {
105 : // send velocity to EKF
106 2886 : AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
107 : }
108 :
109 : // record time for health monitoring
110 2886 : _last_update_ms = AP_HAL::millis();
111 :
112 : #if HAL_LOGGING_ENABLED
113 2886 : Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, _frontend.get_vel_noise(), reset_counter, !consume, _quality);
114 : #endif
115 2886 : }
116 :
117 : // apply rotation to velocity
118 2886 : void AP_VisualOdom_IntelT265::rotate_velocity(Vector3f &velocity) const
119 : {
120 2886 : if (_use_posvel_rotation) {
121 2376 : velocity = _posvel_rotation * velocity;
122 : }
123 2886 : }
124 :
125 : // rotate attitude using _yaw_trim
126 6704 : void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const
127 : {
128 : // apply orientation rotation
129 6704 : if (_use_att_rotation) {
130 0 : attitude *= _att_rotation;
131 : }
132 :
133 : // apply earth-frame yaw rotation
134 6704 : if (!is_zero(_yaw_trim)) {
135 5521 : attitude = _yaw_rotation * attitude;
136 : }
137 6704 : return;
138 : }
139 :
140 : // use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
141 1186 : bool AP_VisualOdom_IntelT265::align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude)
142 : {
143 : // do not align to ahrs if we are its yaw source
144 1186 : if (AP::ahrs().using_extnav_for_yaw()) {
145 0 : return false;
146 : }
147 :
148 : // do not align until ahrs yaw initialised
149 1186 : if (!AP::ahrs().initialised()
150 : #if AP_AHRS_DCM_ENABLED
151 1186 : || !AP::ahrs().dcm_yaw_initialised()
152 : #endif
153 : ) {
154 1183 : return false;
155 : }
156 :
157 3 : align_yaw(position, attitude, AP::ahrs().get_yaw_rad());
158 3 : return true;
159 : }
160 :
161 : // align sensor yaw with any new yaw (in radians)
162 3 : void AP_VisualOdom_IntelT265::align_yaw(const Vector3f &position, const Quaternion &attitude, float yaw_rad)
163 : {
164 : // clear any existing errors
165 3 : _error_orientation = false;
166 :
167 : // create rotation quaternion to correct for orientation
168 3 : const Rotation rot = _frontend.get_orientation();
169 3 : _att_rotation.initialise();
170 3 : _use_att_rotation = false;
171 3 : if (rot != Rotation::ROTATION_NONE) {
172 0 : _att_rotation.rotate(rot);
173 0 : _att_rotation.invert();
174 0 : _use_att_rotation = true;
175 : }
176 :
177 3 : Quaternion att_corrected = attitude;
178 3 : att_corrected *= _att_rotation;
179 :
180 : // extract sensor's corrected yaw
181 3 : const float sens_yaw = att_corrected.get_euler_yaw();
182 :
183 : // trim yaw by difference between ahrs and sensor yaw
184 3 : const float yaw_trim_orig = _yaw_trim;
185 3 : _yaw_trim = wrap_2PI(yaw_rad - sens_yaw);
186 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VisOdom: yaw shifted %d to %d deg",
187 : (int)degrees(_yaw_trim - yaw_trim_orig),
188 : (int)wrap_360(degrees(sens_yaw + _yaw_trim)));
189 :
190 : // convert _yaw_trim to _yaw_rotation to speed up processing later
191 3 : _yaw_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
192 :
193 : // calculate position with current rotation and correction
194 3 : Vector3f pos_orig = position;
195 3 : rotate_and_correct_position(pos_orig);
196 :
197 : // create position and velocity rotation from yaw trim
198 3 : _use_posvel_rotation = false;
199 3 : if (!is_zero(_yaw_trim)) {
200 3 : _posvel_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
201 3 : _use_posvel_rotation = true;
202 : }
203 :
204 : // recalculate position with new rotation
205 3 : Vector3f pos_new = position;
206 3 : rotate_and_correct_position(pos_new);
207 :
208 : // update position correction to remove change due to rotation
209 3 : _pos_correction += (pos_orig - pos_new);
210 3 : }
211 :
212 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
213 115 : bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
214 : {
215 : // exit immediately if not healthy
216 115 : if (!healthy()) {
217 0 : hal.util->snprintf(failure_msg, failure_msg_len, "not healthy");
218 0 : return false;
219 : }
220 :
221 : // check for unsupported orientation
222 115 : if (_error_orientation) {
223 0 : hal.util->snprintf(failure_msg, failure_msg_len, "check VISO_ORIENT parameter");
224 0 : return false;
225 : }
226 :
227 : // get ahrs attitude
228 115 : Quaternion ahrs_quat;
229 115 : if (!AP::ahrs().get_quaternion(ahrs_quat)) {
230 0 : hal.util->snprintf(failure_msg, failure_msg_len, "waiting for AHRS attitude");
231 0 : return false;
232 : }
233 :
234 : // check if roll and pitch is different by > 10deg (using NED so cannot determine whether roll or pitch specifically)
235 115 : const float rp_diff_deg = degrees(ahrs_quat.roll_pitch_difference(_attitude_last));
236 115 : if (rp_diff_deg > 10.0f) {
237 0 : hal.util->snprintf(failure_msg, failure_msg_len, "roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
238 0 : return false;
239 : }
240 :
241 : // check if yaw is different by > 10deg
242 115 : Vector3f angle_diff;
243 115 : ahrs_quat.angular_difference(_attitude_last).to_axis_angle(angle_diff);
244 115 : const float yaw_diff_deg = degrees(fabsf(angle_diff.z));
245 115 : if (yaw_diff_deg > 10.0f) {
246 35 : hal.util->snprintf(failure_msg, failure_msg_len, "yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
247 35 : return false;
248 : }
249 :
250 80 : return true;
251 : }
252 :
253 : // returns true if sensor data should be consumed, false if it should be ignored
254 : // set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise
255 : // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored
256 9590 : bool AP_VisualOdom_IntelT265::should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter)
257 : {
258 9590 : if (get_type() == AP_VisualOdom::VisualOdom_Type::VOXL) {
259 : // we don't discard data after a reset for VOXL
260 0 : return true;
261 : }
262 :
263 9590 : uint32_t now_ms = AP_HAL::millis();
264 :
265 : // set ignore start time if reset counter has changed
266 9590 : if (vision_position_estimate) {
267 6704 : if (reset_counter != _pos_reset_counter_last) {
268 0 : _pos_reset_counter_last = reset_counter;
269 0 : _pos_reset_ignore_start_ms = now_ms;
270 : }
271 : }
272 :
273 : // check if 1 second has passed since the last reset
274 9590 : if ((now_ms - _pos_reset_ignore_start_ms) > VISUALODOM_RESET_IGNORE_DURATION_MS) {
275 9590 : _pos_reset_ignore_start_ms = 0;
276 : }
277 :
278 9590 : return (_pos_reset_ignore_start_ms == 0);
279 : }
280 :
281 : // record voxl camera's position and reset counter for reset jump handling
282 : // position is post scaling, offset and orientation corrections
283 6704 : void AP_VisualOdom_IntelT265::record_voxl_position_and_reset_count(const Vector3f &position, uint8_t reset_counter)
284 : {
285 : // return immediately if not using VOXL camera
286 6704 : if (get_type() != AP_VisualOdom::VisualOdom_Type::VOXL) {
287 6704 : return;
288 : }
289 :
290 0 : _voxl_position_last = position;
291 0 : _voxl_reset_counter_last = reset_counter;
292 : }
293 :
294 : // handle voxl camera reset jumps in attitude and position
295 : // sensor_pos should be the position directly from the sensor with only scaling applied (i.e. no yaw or position corrections)
296 : // sensor_att is similarly the attitude directly from the sensor
297 6704 : void AP_VisualOdom_IntelT265::handle_voxl_camera_reset_jump(const Vector3f &sensor_pos, const Quaternion &sensor_att, uint8_t reset_counter)
298 : {
299 : // return immediately if not using VOXL camera
300 6704 : if (get_type() != AP_VisualOdom::VisualOdom_Type::VOXL) {
301 6704 : return;
302 : }
303 :
304 : // return immediately if no change in reset counter
305 0 : if (reset_counter == _voxl_reset_counter_last) {
306 0 : return;
307 : }
308 :
309 : // warng user of reset
310 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "VisOdom: reset");
311 :
312 : // align sensor yaw to match current yaw estimate
313 0 : align_yaw_to_ahrs(sensor_pos, sensor_att);
314 :
315 : // align psoition to match last recorded position
316 0 : align_position(sensor_pos, _voxl_position_last, true, true);
317 :
318 : // record change in reset counter
319 0 : _voxl_reset_counter_last = reset_counter;
320 : }
321 :
322 : #endif // AP_VISUALODOM_INTELT265_ENABLED
|
