fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FailureDetector - Add check for non-zero roll/pitch failure parameters. Rename 'result' into 'updated'

Browse Source
This commit is contained in:
bresch 2018-07-30 13:40:39 +02:00 committed by Daniel Agar
parent 246b3ebc23
commit 4e1027f292
1 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -56,17 +56,17 @@ FailureDetector::update_params()
bool
FailureDetector::update()
{
bool result(false);
bool updated(false);
result = update_attitude_status();
updated = update_attitude_status();
return result;
return updated;
}
bool
FailureDetector::update_attitude_status()
{
bool result(false);
bool updated(false);
if (_sub_vehicule_attitude.update()) {
const vehicle_attitude_s &attitude = _sub_vehicule_attitude.get();
@ -80,23 +80,29 @@ FailureDetector::update_attitude_status()
const float max_roll(fabsf(math::radians(max_roll_deg)));
const float max_pitch(fabsf(math::radians(max_pitch_deg)));
if (fabsf(roll) > max_roll) {
if ((max_roll > 0.0f) &&
(fabsf(roll) > max_roll)) {
_status.roll = true;
} else {
_status.roll = false;
}
if (fabsf(pitch) > max_pitch) {
if ((max_pitch > 0.0f) &&
(fabsf(pitch) > max_pitch)) {
_status.pitch = true;
} else {
_status.pitch = false;
}
result = true;
updated = true;
} else {
result = false;
updated = false;
}
return result;
return updated;
}