2020-04-03 01:38:17 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "AP_VisualOdom_IntelT265.h"
|
2020-04-06 00:17:42 -03:00
|
|
|
|
|
|
|
#if HAL_VISUALODOM_ENABLED
|
|
|
|
|
2020-04-03 01:38:17 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
// consume vision position estimate data and send to EKF. distances in meters
|
2020-04-10 01:36:26 -03:00
|
|
|
void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
|
2020-04-03 01:38:17 -03:00
|
|
|
{
|
2020-04-09 21:41:14 -03:00
|
|
|
const float scale_factor = _frontend.get_pos_scale();
|
|
|
|
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
|
2020-04-03 01:38:17 -03:00
|
|
|
Quaternion att = attitude;
|
|
|
|
|
|
|
|
// handle user request to align camera
|
|
|
|
if (_align_camera) {
|
|
|
|
if (align_sensor_to_vehicle(pos, attitude)) {
|
|
|
|
_align_camera = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// rotate position and attitude to align with vehicle
|
|
|
|
rotate_and_correct_position(pos);
|
|
|
|
rotate_attitude(att);
|
|
|
|
|
|
|
|
// send attitude and position to EKF
|
|
|
|
const float posErr = 0; // parameter required?
|
|
|
|
const float angErr = 0; // parameter required?
|
2020-04-10 01:36:26 -03:00
|
|
|
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
|
2020-04-03 01:38:17 -03:00
|
|
|
|
|
|
|
// calculate euler orientation for logging
|
|
|
|
float roll;
|
|
|
|
float pitch;
|
|
|
|
float yaw;
|
|
|
|
att.to_euler(roll, pitch, yaw);
|
|
|
|
|
|
|
|
// log sensor data
|
2020-04-10 01:36:26 -03:00
|
|
|
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter);
|
2020-04-03 01:38:17 -03:00
|
|
|
|
|
|
|
// store corrected attitude for use in pre-arm checks
|
|
|
|
_attitude_last = att;
|
|
|
|
|
|
|
|
// record time for health monitoring
|
|
|
|
_last_update_ms = AP_HAL::millis();
|
|
|
|
}
|
|
|
|
|
|
|
|
// apply rotation and correction to position
|
|
|
|
void AP_VisualOdom_IntelT265::rotate_and_correct_position(Vector3f &position) const
|
|
|
|
{
|
|
|
|
if (_use_pos_rotation) {
|
|
|
|
position = _pos_rotation * position;
|
|
|
|
}
|
|
|
|
position += _pos_correction;
|
|
|
|
}
|
|
|
|
|
|
|
|
// rotate attitude using _yaw_trim
|
|
|
|
void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const
|
|
|
|
{
|
|
|
|
// apply orientation rotation
|
|
|
|
if (_use_att_rotation) {
|
|
|
|
attitude *= _att_rotation;
|
|
|
|
}
|
|
|
|
|
|
|
|
// apply earth-frame yaw rotation
|
|
|
|
if (!is_zero(_yaw_trim)) {
|
|
|
|
attitude = _yaw_rotation * attitude;
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
|
|
|
|
bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude)
|
|
|
|
{
|
|
|
|
// fail immediately if ahrs cannot provide attitude
|
|
|
|
Quaternion ahrs_quat;
|
|
|
|
if (!AP::ahrs().get_quaternion(ahrs_quat)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// if ahrs's yaw is from the compass, wait until it has been initialised
|
|
|
|
if (!AP::ahrs().is_ext_nav_used_for_yaw() && !AP::ahrs().yaw_initialised()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// clear any existing errors
|
|
|
|
_error_orientation = false;
|
|
|
|
|
|
|
|
// create rotation quaternion to correct for orientation
|
|
|
|
const Rotation rot = _frontend.get_orientation();
|
|
|
|
_att_rotation.initialise();
|
|
|
|
_use_att_rotation = false;
|
|
|
|
if (rot != Rotation::ROTATION_NONE) {
|
|
|
|
_att_rotation.rotate(rot);
|
|
|
|
_att_rotation.invert();
|
|
|
|
_use_att_rotation = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
Quaternion att_corrected = attitude;
|
|
|
|
att_corrected *= _att_rotation;
|
|
|
|
|
|
|
|
// extract sensor's corrected yaw
|
|
|
|
const float sens_yaw = att_corrected.get_euler_yaw();
|
|
|
|
|
|
|
|
// trim yaw by difference between ahrs and sensor yaw
|
|
|
|
Vector3f angle_diff;
|
|
|
|
ahrs_quat.angular_difference(att_corrected).to_axis_angle(angle_diff);
|
2020-04-09 05:43:47 -03:00
|
|
|
const float yaw_trim_orig = _yaw_trim;
|
2020-04-03 01:38:17 -03:00
|
|
|
_yaw_trim = angle_diff.z;
|
2020-04-07 01:12:15 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "VisualOdom: yaw shifted %d to %d deg",
|
2020-04-09 05:43:47 -03:00
|
|
|
(int)degrees(_yaw_trim - yaw_trim_orig),
|
|
|
|
(int)wrap_360(degrees(sens_yaw + _yaw_trim)));
|
2020-04-03 01:38:17 -03:00
|
|
|
|
|
|
|
// convert _yaw_trim to _yaw_rotation to speed up processing later
|
|
|
|
_yaw_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
|
|
|
|
|
|
|
|
// calculate position with current rotation and correction
|
|
|
|
Vector3f pos_orig = position;
|
|
|
|
rotate_and_correct_position(pos_orig);
|
|
|
|
|
|
|
|
// create position rotation from yaw trim
|
|
|
|
_use_pos_rotation = false;
|
|
|
|
if (!is_zero(_yaw_trim)) {
|
|
|
|
_pos_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
|
|
|
|
_use_pos_rotation = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// recalculate position with new rotation
|
|
|
|
Vector3f pos_new = position;
|
|
|
|
rotate_and_correct_position(pos_new);
|
|
|
|
|
|
|
|
// update position correction to remove change due to rotation
|
|
|
|
_pos_correction += (pos_orig - pos_new);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
|
|
|
|
bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
|
|
|
|
{
|
|
|
|
// exit immediately if not healthy
|
|
|
|
if (!healthy()) {
|
2020-04-07 01:12:15 -03:00
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "not healthy");
|
2020-04-03 01:38:17 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check for unsupported orientation
|
|
|
|
if (_error_orientation) {
|
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "check VISO_ORIENT parameter");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// get ahrs attitude
|
|
|
|
Quaternion ahrs_quat;
|
|
|
|
if (!AP::ahrs().get_quaternion(ahrs_quat)) {
|
2020-04-07 01:12:15 -03:00
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "waiting for AHRS attitude");
|
2020-04-03 01:38:17 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// get angular difference between AHRS and camera attitude
|
|
|
|
Vector3f angle_diff;
|
|
|
|
_attitude_last.angular_difference(ahrs_quat).to_axis_angle(angle_diff);
|
|
|
|
|
|
|
|
// check if roll and pitch is different by > 10deg (using NED so cannot determine whether roll or pitch specifically)
|
|
|
|
const float rp_diff_deg = degrees(safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)));
|
|
|
|
if (rp_diff_deg > 10.0f) {
|
2020-04-07 01:12:15 -03:00
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
|
2020-04-03 01:38:17 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if yaw is different by > 10deg
|
|
|
|
const float yaw_diff_deg = degrees(fabsf(angle_diff.z));
|
|
|
|
if (yaw_diff_deg > 10.0f) {
|
2020-04-07 01:12:15 -03:00
|
|
|
hal.util->snprintf(failure_msg, failure_msg_len, "yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
|
2020-04-03 01:38:17 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
2020-04-06 00:17:42 -03:00
|
|
|
|
|
|
|
#endif
|