34 #include "../PreFlightCheck.hpp" 39 #include <px4_defines.h> 48 const bool optional,
const bool dynamic, int32_t &
device_id,
const bool report_fail)
51 bool calibration_valid =
false;
52 bool accel_valid =
true;
66 device_id = accel.get().device_id;
68 calibration_valid = check_calibration(
"CAL_ACC%u_ID", device_id);
70 if (!calibration_valid) {
78 const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
79 + accel.get().y * accel.get().y
80 + accel.get().z * accel.get().z);
82 if (accel_magnitude < 4.0f || accel_magnitude > 15.0
f ) {
94 if (!optional && report_fail) {
99 const bool success = calibration_valid && accel_valid;
102 set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status);
104 }
else if (instance == 1) {
105 set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status);
static orb_advert_t * mavlink_log_pub
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
static struct vehicle_status_s status
int orb_exists(const struct orb_metadata *meta, int instance)
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
Contains helper functions to efficiently set the system health flags from commander and preflight che...