mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 16:03:33 +08:00
FailureDetector - Add check for non-zero roll/pitch failure parameters. Rename 'result' into 'updated'
This commit is contained in:
@@ -56,17 +56,17 @@ FailureDetector::update_params()
|
|||||||
bool
|
bool
|
||||||
FailureDetector::update()
|
FailureDetector::update()
|
||||||
{
|
{
|
||||||
bool result(false);
|
bool updated(false);
|
||||||
|
|
||||||
result = update_attitude_status();
|
updated = update_attitude_status();
|
||||||
|
|
||||||
return result;
|
return updated;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
FailureDetector::update_attitude_status()
|
FailureDetector::update_attitude_status()
|
||||||
{
|
{
|
||||||
bool result(false);
|
bool updated(false);
|
||||||
|
|
||||||
if (_sub_vehicule_attitude.update()) {
|
if (_sub_vehicule_attitude.update()) {
|
||||||
const vehicle_attitude_s &attitude = _sub_vehicule_attitude.get();
|
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_roll(fabsf(math::radians(max_roll_deg)));
|
||||||
const float max_pitch(fabsf(math::radians(max_pitch_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;
|
_status.roll = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
_status.roll = false;
|
_status.roll = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fabsf(pitch) > max_pitch) {
|
if ((max_pitch > 0.0f) &&
|
||||||
|
(fabsf(pitch) > max_pitch)) {
|
||||||
|
|
||||||
_status.pitch = true;
|
_status.pitch = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
_status.pitch = false;
|
_status.pitch = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
result = true;
|
updated = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
result = false;
|
updated = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return updated;
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user