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

AP_OpticalFlow: Add support for fixed height optical flow

This commit is contained in:
Stephen Dade 2022-10-24 14:01:10 +11:00 committed by Randy Mackay
parent abcacec25f
commit e6b3028007
2 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -93,6 +93,15 @@ const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_ADDR", 5, AP_OpticalFlow, _address, 0), 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 AP_GROUPEND
}; };
@ -259,7 +268,8 @@ void AP_OpticalFlow::update_state(const OpticalFlow_state &state)
_state.flowRate, _state.flowRate,
_state.bodyRate, _state.bodyRate,
_last_update_ms, _last_update_ms,
get_pos_offset()); get_pos_offset(),
get_height_override());
Log_Write_Optflow(); Log_Write_Optflow();
} }

4
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -100,6 +100,9 @@ public:
// last_update() - returns system time of last sensor update // last_update() - returns system time of last sensor update
uint32_t last_update() const { return _last_update_ms; } 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 { struct OpticalFlow_state {
uint8_t surface_quality; // image quality (below TBD you can't trust the dx,dy values returned) 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. 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_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_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_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: // method called by backend to update frontend state:
void update_state(const OpticalFlow_state &state); void update_state(const OpticalFlow_state &state);