diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 2687012e58..f4bf5bb0cd 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -35,6 +35,25 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = { // @User: Standard AP_GROUPINFO("_ORIENT_YAW", 3, OpticalFlow, _yawAngle_cd, 0), + // @Param: _POS_X + // @DisplayName: X position offset + // @Description: X position of the optical flow sensor focal point in body frame. + // @Units: m + // @User: Advanced + + // @Param: _POS_Y + // @DisplayName: Y position offset + // @Description: Y position of the optical flow sensor focal point in body frame. + // @Units: m + // @User: Advanced + + // @Param: _POS_Z + // @DisplayName: Z position offset + // @Description: Z position of the optical flow sensor focal point in body frame. + // @Units: m + // @User: Advanced + AP_GROUPINFO("_POS", 4, OpticalFlow, _pos_offset, 0.0f), + AP_GROUPEND }; diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index b36dbc2cb2..a056a4e771 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -73,6 +73,11 @@ public: // support for HIL/SITL void setHIL(const struct OpticalFlow_state &state); + // return a 3D vector defining the position offset of the sensors focal point in metres relative to the body frame origin + const Vector3f get_pos_offset(void) const { + return _pos_offset; + } + private: OpticalFlow_backend *backend; @@ -85,7 +90,7 @@ private: AP_Int16 _flowScalerX; // X axis flow scale factor correction - parts per thousand AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand AP_Int16 _yawAngle_cd; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees - + AP_Vector3f _pos_offset; // position offset of the flow sensor in the body frame // state filled in by backend struct OpticalFlow_state _state;