diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index b9aa1914b8..5682d5000c 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -93,6 +93,15 @@ const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = { // @User: Advanced AP_GROUPINFO("_ADDR", 5, AP_OpticalFlow, _address, 0), + // @Param: _HGT_OVR + // @DisplayName: Height override of sensor above ground + // @Description: This is used in rover vehicles, where the sensor is a fixed height above the ground + // @Units: m + // @Range: 0 2 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO_FRAME("_HGT_OVR", 6, AP_OpticalFlow, _height_override, 0.0f, AP_PARAM_FRAME_ROVER), + AP_GROUPEND }; @@ -259,7 +268,8 @@ void AP_OpticalFlow::update_state(const OpticalFlow_state &state) _state.flowRate, _state.bodyRate, _last_update_ms, - get_pos_offset()); + get_pos_offset(), + get_height_override()); Log_Write_Optflow(); } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 67de7287c8..b5751d7351 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -100,6 +100,9 @@ public: // last_update() - returns system time of last sensor update uint32_t last_update() const { return _last_update_ms; } + // get_height_override() - returns the user-specified height of sensor above ground + float get_height_override() const { return _height_override; } + struct OpticalFlow_state { uint8_t surface_quality; // image quality (below TBD you can't trust the dx,dy values returned) Vector2f flowRate; // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate. @@ -135,6 +138,7 @@ private: 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 AP_Int8 _address; // address on the bus (allows selecting between 8 possible I2C addresses for px4flow) + AP_Float _height_override; // height of the sensor above the ground. Only used in rover // method called by backend to update frontend state: void update_state(const OpticalFlow_state &state);