mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
AP_VisualOdom: add handle_vision_position_estimate
also add pre_arm_check
This commit is contained in:
parent
1cf9655b4a
commit
0eb1ef1f08
@ -147,6 +147,73 @@ void AP_VisualOdom::handle_msg(const mavlink_message_t &msg)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// general purpose method to consume position estimate data and send to EKF
|
||||||
|
// distances in meters, roll, pitch and yaw are in radians
|
||||||
|
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw)
|
||||||
|
{
|
||||||
|
// exit immediately if not enabled
|
||||||
|
if (!enabled()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// call backend
|
||||||
|
if (_driver != nullptr) {
|
||||||
|
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, roll, pitch, yaw);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// general purpose method to consume position estimate data and send to EKF
|
||||||
|
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
|
||||||
|
{
|
||||||
|
// exit immediately if not enabled
|
||||||
|
if (!enabled()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// call backend
|
||||||
|
if (_driver != nullptr) {
|
||||||
|
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
|
||||||
|
void AP_VisualOdom::align_sensor_to_vehicle()
|
||||||
|
{
|
||||||
|
// exit immediately if not enabled
|
||||||
|
if (!enabled()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// call backend
|
||||||
|
if (_driver != nullptr) {
|
||||||
|
_driver->align_sensor_to_vehicle();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
|
||||||
|
bool AP_VisualOdom::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
|
||||||
|
{
|
||||||
|
// exit immediately if not enabled
|
||||||
|
if (!enabled()) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check healthy
|
||||||
|
if (!healthy()) {
|
||||||
|
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom not healthy");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// if no backend we must have failed to create because out of memory
|
||||||
|
if (_driver == nullptr) {
|
||||||
|
hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom out of memory");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// call backend specific arming check
|
||||||
|
return _driver->pre_arm_check(failure_msg, failure_msg_len);
|
||||||
|
}
|
||||||
|
|
||||||
// singleton instance
|
// singleton instance
|
||||||
AP_VisualOdom *AP_VisualOdom::_singleton;
|
AP_VisualOdom *AP_VisualOdom::_singleton;
|
||||||
|
|
||||||
|
@ -70,6 +70,17 @@ public:
|
|||||||
// consume data from MAVLink messages
|
// consume data from MAVLink messages
|
||||||
void handle_msg(const mavlink_message_t &msg);
|
void handle_msg(const mavlink_message_t &msg);
|
||||||
|
|
||||||
|
// 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, float roll, float pitch, float yaw);
|
||||||
|
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
|
||||||
|
|
||||||
|
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
|
||||||
|
void align_sensor_to_vehicle();
|
||||||
|
|
||||||
|
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
|
||||||
|
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
|
||||||
|
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
@ -14,6 +14,10 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "AP_VisualOdom_Backend.h"
|
#include "AP_VisualOdom_Backend.h"
|
||||||
|
#include <AP_Logger/AP_Logger.h>
|
||||||
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL &hal;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
base class constructor.
|
base class constructor.
|
||||||
@ -39,3 +43,182 @@ void AP_VisualOdom_Backend::set_deltas(const Vector3f &angle_delta, const Vector
|
|||||||
_frontend._state.confidence = confidence;
|
_frontend._state.confidence = confidence;
|
||||||
_frontend._state.last_sensor_update_ms = AP_HAL::millis();
|
_frontend._state.last_sensor_update_ms = AP_HAL::millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// general purpose method to send position estimate data to EKF
|
||||||
|
// distances in meters, roll, pitch and yaw are in radians
|
||||||
|
void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw)
|
||||||
|
{
|
||||||
|
Quaternion attitude;
|
||||||
|
attitude.from_euler(roll, pitch, yaw);
|
||||||
|
handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
@ -16,6 +16,7 @@
|
|||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
#include "AP_VisualOdom.h"
|
#include "AP_VisualOdom.h"
|
||||||
|
|
||||||
class AP_VisualOdom_Backend
|
class AP_VisualOdom_Backend
|
||||||
@ -27,13 +28,43 @@ public:
|
|||||||
// consume VISION_POSITION_DELTA MAVLink message
|
// consume VISION_POSITION_DELTA MAVLink message
|
||||||
virtual void handle_msg(const mavlink_message_t &msg) {};
|
virtual void handle_msg(const mavlink_message_t &msg) {};
|
||||||
|
|
||||||
|
// 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, float roll, float pitch, float yaw);
|
||||||
|
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
|
||||||
|
|
||||||
|
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
|
||||||
|
void align_sensor_to_vehicle() { _align_camera = true; }
|
||||||
|
|
||||||
|
// arming check
|
||||||
|
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
// set deltas (used by backend to update state)
|
// set deltas (used by backend to update state)
|
||||||
void set_deltas(const Vector3f &angle_delta, const Vector3f& position_delta, uint64_t time_delta_usec, float confidence);
|
void set_deltas(const Vector3f &angle_delta, const Vector3f& position_delta, uint64_t time_delta_usec, float confidence);
|
||||||
|
|
||||||
|
// apply rotation and correction to position
|
||||||
|
void rotate_and_correct_position(Vector3f &position) const;
|
||||||
|
|
||||||
|
// rotate attitude using _yaw_trim
|
||||||
|
void rotate_attitude(Quaternion &attitude) const;
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
// references
|
AP_VisualOdom &_frontend; // reference to frontend
|
||||||
AP_VisualOdom &_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)
|
||||||
};
|
};
|
||||||
|
Loading…
Reference in New Issue
Block a user