Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: integrate IntelT265 backend

Browse Source
This commit is contained in:
Randy Mackay 2020-04-03 13:38:37 +09:00
parent f0e11d9a5c
commit 8f94a0cfb0
4 changed files with 23 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -16,6 +16,7 @@
#include "AP_VisualOdom.h"
#include "AP_VisualOdom_Backend.h"
#include "AP_VisualOdom_MAV.h"
#include "AP_VisualOdom_IntelT265.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
@ -27,7 +28,7 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
// @Param: _TYPE
// @DisplayName: Visual odometry camera connection type
// @Description: Visual odometry camera connection type
// @Values: 0:None,1:MAV
// @Values: 0:None,1:MAV,2:IntelT265
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FLAGS("_TYPE", 0, AP_VisualOdom, _type, 0, AP_PARAM_FLAG_ENABLE),
@ -82,8 +83,16 @@ AP_VisualOdom::AP_VisualOdom()
void AP_VisualOdom::init()
{
// create backend
if (_type == AP_VisualOdom_Type_MAV) {
switch (_type) {
case AP_VisualOdom_Type_None:
// do nothing
break;
case AP_VisualOdom_Type_MAV:
_driver = new AP_VisualOdom_MAV(*this);
break;
case AP_VisualOdom_Type_IntelT265:
_driver = new AP_VisualOdom_IntelT265(*this);
break;
}
}

5
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -36,8 +36,9 @@ public:
// external position backend types (used by _TYPE parameter)
enum AP_VisualOdom_Type {
AP_VisualOdom_Type_None = 0,
AP_VisualOdom_Type_MAV = 1
AP_VisualOdom_Type_None = 0,
AP_VisualOdom_Type_MAV = 1,
AP_VisualOdom_Type_IntelT265 = 2
};
// detect and initialise any sensors

173
libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
View File

@ -34,176 +34,3 @@ bool AP_VisualOdom_Backend::healthy() const
// healthy if we have received sensor messages within the past 300ms
return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
}
// general purpose method to send position estimate data to EKF
void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
{
Vector3f pos = {x, y, z};
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?
const uint32_t reset_timestamp_ms = 0; // no data available
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, reset_timestamp_ms);
// calculate euler orientation for logging
float roll;
float pitch;
float yaw;
att.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw));
// store corrected attitude for use in pre-arm checks
_attitude_last = att;
_have_attitude = true;
// record time for health monitoring
_last_update_ms = AP_HAL::millis();
}
// apply rotation and correction to position
void AP_VisualOdom_Backend::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_Backend::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_Backend::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);
_yaw_trim = angle_diff.z;
gcs().send_text(MAV_SEVERITY_CRITICAL, "VisOdom: yaw shifted %d to %d deg",
//(int)degrees(_yaw_trim - yaw_trim_orig),
(int)degrees(_yaw_trim),
(int)degrees(sens_yaw + _yaw_trim));
// 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)) {
Matrix3f trim_matrix;
_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_Backend::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
// exit immediately if no attitude to check
if (!_have_attitude) {
return true;
}
// 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)) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom waiting for AHRS attitude");
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) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
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) {
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
return false;
}
return true;
}

31
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -29,17 +29,16 @@ public:
bool healthy() const;
// consume vision_position_delta mavlink messages
virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) {};
virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) = 0;
// general purpose methods to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
// consume vision position estimate data and send to EKF. distances in meters
virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) = 0;
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
void align_sensor_to_vehicle() { _align_camera = true; }
// handle request to align camera's attitude with vehicle's AHRS/EKF attitude
virtual void align_sensor_to_vehicle() {}
// arming check
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// arming check - by default no checks performed
virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; }
protected:
@ -52,18 +51,6 @@ protected:
// use sensor provided position and attitude to calculate rotation to align sensor with AHRS/EKF attitude
bool align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude);
AP_VisualOdom &_frontend; // reference to frontend
float _yaw_trim; // yaw angle trim (in radians) to align camera's yaw to ahrs/EKF's
Quaternion _yaw_rotation; // earth-frame yaw rotation to align heading of sensor with vehicle. use when _yaw_trim is non-zero
Quaternion _att_rotation; // body-frame rotation corresponding to ORIENT parameter. use when get_orientation != NONE
Matrix3f _pos_rotation; // rotation to align position from sensor to earth frame. use when _use_pos_rotation is true
Vector3f _pos_correction; // position correction that should be added to position reported from sensor
bool _use_att_rotation; // true if _att_rotation should be applied to sensor's attitude data
bool _use_pos_rotation; // true if _pos_rotation should be applied to sensor's position data
bool _align_camera = true; // true if camera should be aligned to AHRS/EKF
bool _have_attitude; // true if we have received an attitude from the camera (used for arming checks)
bool _error_orientation; // true if the orientation is not supported
Quaternion _attitude_last; // last attitude received from camera (used for arming checks)
uint32_t _last_update_ms; // system time of last update from sensor (used by health checks)
AP_VisualOdom &_frontend; // reference to frontend
uint32_t _last_update_ms; // system time of last update from sensor (used by health checks)
};