Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Improve optical flow takeoff detection logic

Browse Source 
Ensure takeoff detect status goes to false when on ground
This commit is contained in:
Paul Riseborough 2015-10-14 10:06:43 +11:00 committed by Andrew Tridgell
parent d3a6690e4f
commit 0dc570b5a5
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -102,8 +102,6 @@ void NavEKF2_core::setAidingMode()
// We have transitioned either into or out of aiding // We have transitioned either into or out of aiding
// zero stored velocities used to do dead-reckoning // zero stored velocities used to do dead-reckoning
heldVelNE.zero(); heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation
takeOffDetected = false;
// set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped. // set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (!isAiding) { if (!isAiding) {
// We have ceased aiding // We have ceased aiding

6
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -401,7 +401,8 @@ void NavEKF2_core::setTouchdownExpected(bool val)
// Detect takeoff for optical flow navigation // Detect takeoff for optical flow navigation
void NavEKF2_core::detectOptFlowTakeoff(void) void NavEKF2_core::detectOptFlowTakeoff(void)
{ {
if (motorsArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
// we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
const AP_InertialSensor &ins = _ahrs->get_ins(); const AP_InertialSensor &ins = _ahrs->get_ins();
Vector3f angRateVec; Vector3f angRateVec;
Vector3f gyroBias; Vector3f gyroBias;
@ -414,6 +415,9 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
} }
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f))); takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f)));
} else if (onGround) {
// we are confidently on the ground so set the takeoff detected status to false
takeOffDetected = false;
} }
} }