mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 12:38:33 -04:00
Plane: check for optflow enable in more places
This commit is contained in:
parent
d018a32545
commit
e8976d3a89
@ -818,8 +818,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
|
|||||||
|
|
||||||
case MSG_OPTICAL_FLOW:
|
case MSG_OPTICAL_FLOW:
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
|
if (plane.optflow.enabled()) {
|
||||||
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
|
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
|
||||||
send_opticalflow(plane.ahrs, plane.optflow);
|
send_opticalflow(plane.ahrs, plane.optflow);
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -179,7 +179,9 @@ void Plane::init_ardupilot()
|
|||||||
|
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
// make optflow available to libraries
|
// make optflow available to libraries
|
||||||
|
if (optflow.enabled()) {
|
||||||
ahrs.set_optflow(&optflow);
|
ahrs.set_optflow(&optflow);
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Register mavlink_delay_cb, which will run anytime you have
|
// Register mavlink_delay_cb, which will run anytime you have
|
||||||
@ -246,11 +248,9 @@ void Plane::init_ardupilot()
|
|||||||
|
|
||||||
// initialise sensor
|
// initialise sensor
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
if (!optflow.enabled()) {
|
if (optflow.enabled()) {
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
optflow.init();
|
optflow.init();
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user