mirror of https://github.com/ArduPilot/ardupilot
AP_VisualOdom: use Odometry quality
This commit is contained in:
Randy Mackay
parent
94150368ee
commit
4fcd8b1068
|
|
@ -106,6 +106,14 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),
|
||||
|
||||
// @Param: _QUAL_MIN
|
||||
// @DisplayName: Visual odometry minimum quality
|
||||
// @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
|
||||
// @Units: %
|
||||
// @Range: -1 100
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_QUAL_MIN", 8, AP_VisualOdom, _quality_min, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -161,6 +169,16 @@ bool AP_VisualOdom::healthy() const
|
|||
return _driver->healthy();
|
||||
}
|
||||
|
||||
// return quality as a measure from 0 ~ 100
|
||||
// -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
int8_t AP_VisualOdom::quality() const
|
||||
{
|
||||
if (_driver == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
return _driver->quality();
|
||||
}
|
||||
|
||||
#if HAL_GCS_ENABLED
|
||||
// consume vision_position_delta mavlink messages
|
||||
void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &msg)
|
||||
|
|
@ -179,7 +197,8 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms
|
|||
|
||||
// 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_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)
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
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)
|
||||
{
|
||||
// exit immediately if not enabled
|
||||
if (!enabled()) {
|
||||
|
|
@ -191,12 +210,13 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
|
|||
// convert attitude to quaternion and call backend
|
||||
Quaternion attitude;
|
||||
attitude.from_euler(roll, pitch, yaw);
|
||||
_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
|
||||
_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
|
||||
}
|
||||
}
|
||||
|
||||
// general purpose method to consume position estimate data and send to EKF
|
||||
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)
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
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)
|
||||
{
|
||||
// exit immediately if not enabled
|
||||
if (!enabled()) {
|
||||
|
|
@ -205,11 +225,14 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
|
|||
|
||||
// call backend
|
||||
if (_driver != nullptr) {
|
||||
_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
|
||||
_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
|
||||
// general purpose methods to consume velocity estimate data and send to EKF
|
||||
// velocity in NED meters per second
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
|
||||
{
|
||||
// exit immediately if not enabled
|
||||
if (!enabled()) {
|
||||
|
|
@ -218,7 +241,7 @@ void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32
|
|||
|
||||
// call backend
|
||||
if (_driver != nullptr) {
|
||||
_driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter);
|
||||
_driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -83,6 +83,13 @@ public:
|
|||
// return yaw measurement noise in rad
|
||||
float get_yaw_noise() const { return _yaw_noise; }
|
||||
|
||||
// return quality threshold
|
||||
int8_t get_quality_min() const { return _quality_min; }
|
||||
|
||||
// return quality as a measure from -1 ~ 100
|
||||
// -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
int8_t quality() const;
|
||||
|
||||
#if HAL_GCS_ENABLED
|
||||
// consume vision_position_delta mavlink messages
|
||||
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
|
||||
|
|
@ -90,12 +97,14 @@ public:
|
|||
|
||||
// general purpose methods to consume position estimate data and send to EKF
|
||||
// distances in meters, roll, pitch and yaw are in radians
|
||||
void 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);
|
||||
void 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);
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void 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);
|
||||
void 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);
|
||||
|
||||
// general purpose methods to consume velocity estimate data and send to EKF
|
||||
// velocity in NED meters per second
|
||||
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter);
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);
|
||||
|
||||
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
|
||||
void request_align_yaw_to_ahrs();
|
||||
|
|
@ -126,6 +135,7 @@ private:
|
|||
AP_Float _vel_noise; // velocity measurement noise in m/s
|
||||
AP_Float _pos_noise; // position measurement noise in meters
|
||||
AP_Float _yaw_noise; // yaw measurement noise in radians
|
||||
AP_Int8 _quality_min; // positions and velocities will only be sent to EKF if over this value. if 0 all values sent to EKF
|
||||
|
||||
// reference to backends
|
||||
AP_VisualOdom_Backend *_driver;
|
||||
|
|
|
|||
|
|
@ -29,16 +29,22 @@ public:
|
|||
// return true if sensor is basically healthy (we are receiving data)
|
||||
bool healthy() const;
|
||||
|
||||
// return quality as a measure from -1 ~ 100
|
||||
// -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
int8_t quality() const { return _quality; }
|
||||
|
||||
#if HAL_GCS_ENABLED
|
||||
// consume vision_position_delta mavlink messages
|
||||
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
|
||||
#endif
|
||||
|
||||
// consume vision pose estimate data and send to EKF. distances in meters
|
||||
virtual void 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) = 0;
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
virtual void 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) = 0;
|
||||
|
||||
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
|
||||
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) = 0;
|
||||
|
||||
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
|
||||
virtual void request_align_yaw_to_ahrs() {}
|
||||
|
|
@ -62,8 +68,8 @@ protected:
|
|||
#if HAL_LOGGING_ENABLED
|
||||
// Logging Functions
|
||||
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
|
||||
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored);
|
||||
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored);
|
||||
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality);
|
||||
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality);
|
||||
#endif
|
||||
|
||||
AP_VisualOdom &_frontend; // reference to frontend
|
||||
|
|
@ -72,6 +78,9 @@ protected:
|
|||
// reset counter handling
|
||||
uint8_t _last_reset_counter; // last sensor reset counter received
|
||||
uint32_t _reset_timestamp_ms; // time reset counter was received
|
||||
|
||||
// quality
|
||||
int8_t _quality; // last recorded quality
|
||||
};
|
||||
|
||||
#endif // HAL_VISUALODOM_ENABLED
|
||||
|
|
|
|||
|
|
@ -28,7 +28,8 @@
|
|||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// consume vision pose estimate data and send to EKF. distances in meters
|
||||
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)
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
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)
|
||||
{
|
||||
const float scale_factor = _frontend.get_pos_scale();
|
||||
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
|
||||
|
|
@ -59,8 +60,11 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
|
|||
posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
|
||||
angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
|
||||
|
||||
// record quality
|
||||
_quality = quality;
|
||||
|
||||
// check for recent position reset
|
||||
bool consume = should_consume_sensor_data(true, reset_counter);
|
||||
bool consume = should_consume_sensor_data(true, reset_counter) && (_quality >= _frontend.get_quality_min());
|
||||
if (consume) {
|
||||
// send attitude and position to EKF
|
||||
AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
|
||||
|
|
@ -74,7 +78,7 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
|
|||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
// log sensor data
|
||||
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);
|
||||
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);
|
||||
#endif
|
||||
|
||||
// store corrected attitude for use in pre-arm checks
|
||||
|
|
@ -85,14 +89,18 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
|
|||
}
|
||||
|
||||
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
|
||||
void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
|
||||
{
|
||||
// rotate velocity to align with vehicle
|
||||
Vector3f vel_corrected = vel;
|
||||
rotate_velocity(vel_corrected);
|
||||
|
||||
// record quality
|
||||
_quality = quality;
|
||||
|
||||
// check for recent position reset
|
||||
bool consume = should_consume_sensor_data(false, reset_counter);
|
||||
bool consume = should_consume_sensor_data(false, reset_counter) && (_quality >= _frontend.get_quality_min());
|
||||
if (consume) {
|
||||
// send velocity to EKF
|
||||
AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
|
||||
|
|
@ -102,7 +110,7 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
|
|||
_last_update_ms = AP_HAL::millis();
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume);
|
||||
Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume, _quality);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -14,10 +14,12 @@ public:
|
|||
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
|
||||
|
||||
// consume vision pose estimate data and send to EKF. distances in meters
|
||||
void 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) override;
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void 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) override;
|
||||
|
||||
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
|
||||
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
|
||||
|
||||
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
|
||||
void request_align_yaw_to_ahrs() override { _align_yaw = true; }
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &a
|
|||
}
|
||||
|
||||
// Write visual position sensor data. x,y,z are in meters, angles are in degrees
|
||||
void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored)
|
||||
void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality)
|
||||
{
|
||||
const struct log_VisualPosition pkt_visualpos {
|
||||
LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
|
||||
|
|
@ -40,13 +40,14 @@ void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32
|
|||
pos_err : pos_err,
|
||||
ang_err : ang_err,
|
||||
reset_counter : reset_counter,
|
||||
ignored : (uint8_t)ignored
|
||||
ignored : (uint8_t)ignored,
|
||||
quality : quality
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
|
||||
}
|
||||
|
||||
// Write visual velocity sensor data, velocity in NED meters per second
|
||||
void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored)
|
||||
void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality)
|
||||
{
|
||||
const struct log_VisualVelocity pkt_visualvel {
|
||||
LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
|
||||
|
|
@ -58,7 +59,8 @@ void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32
|
|||
vel_z : vel.z,
|
||||
vel_err : _frontend.get_vel_noise(),
|
||||
reset_counter : reset_counter,
|
||||
ignored : (uint8_t)ignored
|
||||
ignored : (uint8_t)ignored,
|
||||
quality : quality
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,15 +24,23 @@
|
|||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
// consume vision pose estimate data and send to EKF. distances in meters
|
||||
void AP_VisualOdom_MAV::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)
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void AP_VisualOdom_MAV::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)
|
||||
{
|
||||
const float scale_factor = _frontend.get_pos_scale();
|
||||
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
|
||||
|
||||
posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
|
||||
angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
|
||||
// send attitude and position to EKF
|
||||
AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
|
||||
|
||||
// record quality
|
||||
_quality = quality;
|
||||
|
||||
// send attitude and position to EKF if quality OK
|
||||
bool consume = (_quality >= _frontend.get_quality_min());
|
||||
if (consume) {
|
||||
AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
|
||||
}
|
||||
|
||||
// calculate euler orientation for logging
|
||||
float roll;
|
||||
|
|
@ -42,23 +50,31 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t
|
|||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
// log sensor data
|
||||
Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
|
||||
Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, !consume, _quality);
|
||||
#endif
|
||||
|
||||
// record time for health monitoring
|
||||
_last_update_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
|
||||
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
|
||||
{
|
||||
// send velocity to EKF
|
||||
AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
|
||||
// record quality
|
||||
_quality = quality;
|
||||
|
||||
// send velocity to EKF if quality OK
|
||||
bool consume = (_quality >= _frontend.get_quality_min());
|
||||
if (consume) {
|
||||
AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
|
||||
}
|
||||
|
||||
// record time for health monitoring
|
||||
_last_update_ms = AP_HAL::millis();
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false);
|
||||
Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, !consume, _quality);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -14,10 +14,12 @@ public:
|
|||
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
|
||||
|
||||
// consume vision pose estimate data and send to EKF. distances in meters
|
||||
void 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) override;
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void 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) override;
|
||||
|
||||
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
|
||||
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
|
||||
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
|
||||
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
|
||||
};
|
||||
|
||||
#endif // AP_VISUALODOM_MAV_ENABLED
|
||||
|
|
|
|||
|
|
@ -40,13 +40,14 @@ struct PACKED log_VisualOdom {
|
|||
// @Field: PX: Position X-axis (North-South)
|
||||
// @Field: PY: Position Y-axis (East-West)
|
||||
// @Field: PZ: Position Z-axis (Down-Up)
|
||||
// @Field: Roll: Roll lean angle
|
||||
// @Field: Pitch: Pitch lean angle
|
||||
// @Field: Yaw: Yaw angle
|
||||
// @Field: R: Roll lean angle
|
||||
// @Field: P: Pitch lean angle
|
||||
// @Field: Y: Yaw angle
|
||||
// @Field: PErr: Position estimate error
|
||||
// @Field: AErr: Attitude estimate error
|
||||
// @Field: Rst: Position reset counter
|
||||
// @Field: Ign: Ignored
|
||||
// @Field: Q: Quality
|
||||
struct PACKED log_VisualPosition {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
|
|
@ -62,6 +63,7 @@ struct PACKED log_VisualPosition {
|
|||
float ang_err; // radians
|
||||
uint8_t reset_counter;
|
||||
uint8_t ignored;
|
||||
int8_t quality;
|
||||
};
|
||||
|
||||
// @LoggerMessage: VISV
|
||||
|
|
@ -75,6 +77,7 @@ struct PACKED log_VisualPosition {
|
|||
// @Field: VErr: Velocity estimate error
|
||||
// @Field: Rst: Velocity reset counter
|
||||
// @Field: Ign: Ignored
|
||||
// @Field: Q: Quality
|
||||
struct PACKED log_VisualVelocity {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
|
|
@ -86,6 +89,7 @@ struct PACKED log_VisualVelocity {
|
|||
float vel_err;
|
||||
uint8_t reset_counter;
|
||||
uint8_t ignored;
|
||||
int8_t quality;
|
||||
};
|
||||
|
||||
#if HAL_VISUALODOM_ENABLED
|
||||
|
|
@ -93,9 +97,9 @@ struct PACKED log_VisualVelocity {
|
|||
{ LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
|
||||
"VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
|
||||
{ LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
|
||||
"VISP", "QQIffffffffBB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,Rst,Ign", "sssmmmddhmd--", "FFC00000000--" }, \
|
||||
"VISP", "QQIffffffffBBb", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,R,P,Y,PErr,AErr,Rst,Ign,Q", "sssmmmddhmd--%", "FFC00000000--0" }, \
|
||||
{ LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
|
||||
"VISV", "QQIffffBB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign", "sssnnnn--", "FFC0000--" },
|
||||
"VISV", "QQIffffBBb", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign,Q", "sssnnnn--%", "FFC0000--0" },
|
||||
#else
|
||||
#define LOG_STRUCTURE_FROM_VISUALODOM
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue