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

AP_VisualOdom: add SCALE parameter 

Applied to positions from vision-position-estimate messages
This commit is contained in:
Randy Mackay 2020-04-10 09:41:14 +09:00 committed by Andrew Tridgell
parent 852325c8e4
commit 83e48b575e
4 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -68,6 +68,12 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_ORIENT", 2, AP_VisualOdom, _orientation, ROTATION_NONE), AP_GROUPINFO("_ORIENT", 2, AP_VisualOdom, _orientation, ROTATION_NONE),
// @Param: _SCALE
// @DisplayName: Visual odometry scaling factor
// @Description: Visual odometry scaling factor applied to position estimates from sensor
// @User: Advanced
AP_GROUPINFO("_SCALE", 3, AP_VisualOdom, _pos_scale, 1.0f),
AP_GROUPEND AP_GROUPEND
}; };

6
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -62,6 +62,9 @@ public:
// get user defined orientation // get user defined orientation
enum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); } enum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }
// get user defined scaling applied to position estimates
float get_pos_scale() const { return _pos_scale; }
// return a 3D vector defining the position offset of the camera in meters relative to the body frame origin // return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
const Vector3f &get_pos_offset(void) const { return _pos_offset; } const Vector3f &get_pos_offset(void) const { return _pos_offset; }
@ -86,9 +89,10 @@ private:
static AP_VisualOdom *_singleton; static AP_VisualOdom *_singleton;
// parameters // parameters
AP_Int8 _type; AP_Int8 _type; // sensor type
AP_Vector3f _pos_offset; // position offset of the camera in the body frame AP_Vector3f _pos_offset; // position offset of the camera in the body frame
AP_Int8 _orientation; // camera orientation on vehicle frame AP_Int8 _orientation; // camera orientation on vehicle frame
AP_Float _pos_scale; // position scale factor applied to sensor values
// reference to backends // reference to backends
AP_VisualOdom_Backend *_driver; AP_VisualOdom_Backend *_driver;

3
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -26,7 +26,8 @@ extern const AP_HAL::HAL& hal;
// consume vision position estimate data and send to EKF. distances in meters // 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) 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)
{ {
Vector3f pos{x, y, z}; const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
Quaternion att = attitude; Quaternion att = attitude;
// handle user request to align camera // handle user request to align camera

3
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -65,7 +65,8 @@ void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t
// consume vision position estimate data and send to EKF. distances in meters // 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) 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)
{ {
Vector3f pos{x, y, z}; const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
// send attitude and position to EKF // send attitude and position to EKF
const float posErr = 0; // parameter required? const float posErr = 0; // parameter required?