dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Removed debug parts 

Disable free-fall detection when threshold is 0
This commit is contained in:
Emmanuel Roussel 2016-01-28 13:46:41 +01:00 committed by Roman
parent 776c7f5d60
commit 59c9f6b4fb
1 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -118,27 +118,25 @@ LandDetectionResult MulticopterLandDetector::update()
bool MulticopterLandDetector::get_freefall_state()
{
if(_params.acc_threshold_m_s2 < 0.1f || _params.acc_threshold_m_s2 > 10.0f){ //if parameter is set to zero or invalid, disable free-fall detection.
return false;
}
const uint64_t now = hrt_absolute_time();
float acc_norm = 0.0;
for (int i = 0; i < 3; i++) {
acc_norm += _sensors.accelerometer_m_s2[i] * _sensors.accelerometer_m_s2[i];
}
acc_norm = sqrtf(acc_norm); //norm of specific force
acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed.
int32_t elapsed_time_ms = (now - _freefallTimer) / 1000;
bool freefall = (acc_norm < _params.acc_threshold_m_s2);
bool freefall = (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling
if (!freefall || _freefallTimer == 0) { //reset timer if uav not falling
_freefallTimer = now;
return false;
}/* else { //DEBUG
PX4_WARN("[freefalldetector] FREE FALL !! \tacc_norm:\t%8.4f, \telapsed time:\t%8.4f ms", (double)acc_norm, (double)elapsed_time_ms);
}
bool falling = elapsed_time_ms > _params.ff_trigger_time_ms;
if (falling) {
PX4_WARN("[freefalldetector] TRIGGERED");
}*/
return elapsed_time_ms > _params.ff_trigger_time_ms;
return (now - _freefallTimer) / 1000 > _params.ff_trigger_time_ms;
}
bool MulticopterLandDetector::get_landed_state()