Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: support covariance from msg and add pos, ang err parameters

This commit is contained in:
chobits 2020-06-04 10:56:44 +08:00 committed by Randy Mackay
parent 991387dbfb
commit 5d271d1e04
7 changed files with 39 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -90,6 +90,22 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_VEL_M_NSE", 5, AP_VisualOdom, _vel_noise, 0.1),
// @Param: _POS_M_NSE
// @DisplayName: Visual odometry position measurement noise
// @Description: Visual odometry position measurement noise in meters
// @Units: m
// @Range: 0.1 10.0
// @User: Advanced
AP_GROUPINFO("_POS_M_NSE", 6, AP_VisualOdom, _pos_noise, 0.2f),
// @Param: _YAW_M_NSE
// @DisplayName: Visual odometry yaw measurement noise
// @Description: Visual odometry yaw measurement noise in radians
// @Units: rad
// @Range: 0.05 1.0
// @User: Advanced
AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),
AP_GROUPEND
};
@ -156,7 +172,7 @@ 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_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter)
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, float posErr, float angErr, uint8_t reset_counter)
{
// exit immediately if not enabled
if (!enabled()) {
@ -168,7 +184,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// convert attitude to quaternion and call backend
Quaternion attitude;
attitude.from_euler(roll, pitch, yaw);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
}
}
@ -182,7 +198,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// call backend
if (_driver != nullptr) {
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, 0, 0, reset_counter);
}
}

10
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -73,13 +73,19 @@ public:
// return velocity measurement noise in m/s
float get_vel_noise() const { return _vel_noise; }
// return position measurement noise in m
float get_pos_noise() const { return _pos_noise; }
// return yaw measurement noise in rad
float get_yaw_noise() const { return _yaw_noise; }
// consume vision_position_delta mavlink messages
void handle_vision_position_delta_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, uint8_t reset_counter);
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, float posErr, float angErr, uint8_t reset_counter);
void 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);
// general purpose methods to consume velocity estimate data and send to EKF
@ -105,6 +111,8 @@ private:
AP_Float _pos_scale; // position scale factor applied to sensor values
AP_Int16 _delay_ms; // average delay relative to inertial measurements
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
// reference to backends
AP_VisualOdom_Backend *_driver;

2
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -31,7 +31,7 @@ public:
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
// 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, uint8_t reset_counter) = 0;
virtual void handle_vision_position_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;
// 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;

8
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -24,7 +24,7 @@
extern const AP_HAL::HAL& hal;
// consume vision position estimate data and send to EKF. distances in meters
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)
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, float posErr, float angErr, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
@ -41,9 +41,9 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
rotate_and_correct_position(pos);
rotate_attitude(att);
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
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
// calculate euler orientation for logging
@ -53,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
att.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter);
AP::logger().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);
// store corrected attitude for use in pre-arm checks
_attitude_last = att;

2
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
View File

@ -12,7 +12,7 @@ public:
using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
// consume vision position estimate data and send to EKF. distances in meters
void 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) override;
void handle_vision_position_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;
// 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;

8
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -30,14 +30,14 @@ AP_VisualOdom_MAV::AP_VisualOdom_MAV(AP_VisualOdom &frontend) :
}
// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_MAV::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)
void AP_VisualOdom_MAV::handle_vision_position_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)
{
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
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
// calculate euler orientation for logging
@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
attitude.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), reset_counter);
AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter);
// record time for health monitoring
_last_update_ms = AP_HAL::millis();

2
libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
View File

@ -12,7 +12,7 @@ public:
AP_VisualOdom_MAV(AP_VisualOdom &frontend);
// consume vision position estimate data and send to EKF. distances in meters
void 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) override;
void handle_vision_position_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;
// 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;