AP_VisualOdom: add SCALE parameter

Applied to positions from vision-position-estimate messages

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -68,6 +68,12 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
     // @User: Advanced
     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
 };
 

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -62,6 +62,9 @@ public:
     // get user defined orientation
     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
     const Vector3f &get_pos_offset(void) const { return _pos_offset; }
 
@@ -86,9 +89,10 @@ private:
     static AP_VisualOdom *_singleton;
 
     // parameters
-    AP_Int8 _type;
+    AP_Int8 _type;              // sensor type
     AP_Vector3f _pos_offset;    // position offset of the camera in the body frame
     AP_Int8 _orientation;       // camera orientation on vehicle frame
+    AP_Float _pos_scale;        // position scale factor applied to sensor values
 
     // reference to backends
     AP_VisualOdom_Backend *_driver;

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -26,7 +26,8 @@ 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)
 {
-    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;
 
     // handle user request to align camera

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -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
 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
     const float posErr = 0; // parameter required?