34 #include "../PreFlightCheck.hpp" 38 #include <px4_defines.h> 47 const bool optional, int32_t &
device_id,
const bool report_fail)
50 bool calibration_valid =
false;
51 bool gyro_valid =
false;
65 device_id = gyro.get().device_id;
67 calibration_valid = check_calibration(
"CAL_GYRO%u_ID", device_id);
69 if (!calibration_valid) {
76 if (!optional && report_fail) {
82 set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status);
84 }
else if (instance == 1) {
85 set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status);
88 return calibration_valid && gyro_valid;
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.
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail)
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.
Contains helper functions to efficiently set the system health flags from commander and preflight che...