mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 04:13:35 +08:00
Failure Detector - Various cleanup and style changes.
- use pragma once guard in FailureDetector.hpp - send Commander parent to ModuleParams and remove update_params() method - simplify attitude checks - FailureDetector::get_status() (previously named "get") is now a constant method that returns a constant reference
This commit is contained in:
@@ -40,19 +40,13 @@
|
||||
|
||||
#include "FailureDetector.hpp"
|
||||
|
||||
FailureDetector::FailureDetector() :
|
||||
ModuleParams(nullptr),
|
||||
FailureDetector::FailureDetector(ModuleParams *parent) :
|
||||
ModuleParams(parent),
|
||||
_sub_vehicle_attitude_setpoint(ORB_ID(vehicle_attitude_setpoint)),
|
||||
_sub_vehicule_attitude(ORB_ID(vehicle_attitude))
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
FailureDetector::update_params()
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
|
||||
bool
|
||||
FailureDetector::update()
|
||||
{
|
||||
@@ -80,23 +74,8 @@ 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 ((max_roll > 0.0f) &&
|
||||
(fabsf(roll) > max_roll)) {
|
||||
|
||||
_status.roll = true;
|
||||
|
||||
} else {
|
||||
_status.roll = false;
|
||||
}
|
||||
|
||||
if ((max_pitch > 0.0f) &&
|
||||
(fabsf(pitch) > max_pitch)) {
|
||||
|
||||
_status.pitch = true;
|
||||
|
||||
} else {
|
||||
_status.pitch = false;
|
||||
}
|
||||
_status.roll = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
|
||||
_status.pitch = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
|
||||
|
||||
updated = true;
|
||||
|
||||
|
||||
@@ -41,8 +41,7 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef FAILURE_DETECTOR_HPP
|
||||
#define FAILURE_DETECTOR_HPP
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
@@ -53,24 +52,22 @@
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
|
||||
struct failure_detector_status_s {
|
||||
bool roll;
|
||||
bool pitch;
|
||||
bool altitude;
|
||||
};
|
||||
|
||||
using uORB::Subscription;
|
||||
|
||||
class FailureDetector : public ModuleParams
|
||||
{
|
||||
public:
|
||||
FailureDetector();
|
||||
|
||||
void update_params();
|
||||
FailureDetector(ModuleParams *parent);
|
||||
|
||||
bool update();
|
||||
|
||||
struct failure_detector_status_s {
|
||||
bool roll;
|
||||
bool pitch;
|
||||
bool altitude;
|
||||
};
|
||||
|
||||
failure_detector_status_s get() {return _status;};
|
||||
const failure_detector_status_s& get_status() const {return _status;}
|
||||
|
||||
private:
|
||||
|
||||
@@ -87,5 +84,3 @@ private:
|
||||
|
||||
bool update_attitude_status();
|
||||
};
|
||||
|
||||
#endif /* FAILURE_DETECTOR_HPP */
|
||||
|
||||
Reference in New Issue
Block a user