PX4 Firmware
PX4 Autopilot Software http://px4.io
magnetometerCheck.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include "../PreFlightCheck.hpp"
35 
36 #include <drivers/drv_hrt.h>
37 #include <HealthFlags.h>
38 #include <px4_defines.h>
39 #include <systemlib/mavlink_log.h>
40 #include <uORB/Subscription.hpp>
41 #include <uORB/topics/sensor_mag.h>
43 
44 using namespace time_literals;
45 
47  const bool optional, int32_t &device_id, const bool report_fail)
48 {
49  const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
50  bool calibration_valid = false;
51  bool mag_valid = false;
52 
53  if (exists) {
54 
55  uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
56 
57  mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s);
58 
59  if (!mag_valid) {
60  if (report_fail) {
61  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance);
62  }
63  }
64 
65  device_id = magnetometer.get().device_id;
66 
67  calibration_valid = check_calibration("CAL_MAG%u_ID", device_id);
68 
69  if (!calibration_valid) {
70  if (report_fail) {
71  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance);
72  }
73  }
74 
75  } else {
76  if (!optional && report_fail) {
77  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor #%u missing", instance);
78  }
79  }
80 
81  const bool success = calibration_valid && mag_valid;
82 
83  if (instance == 0) {
84  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
85 
86  } else if (instance == 1) {
87  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
88  }
89 
90  return success;
91 }
static orb_advert_t * mavlink_log_pub
static struct vehicle_status_s status
Definition: Commander.cpp:138
LidarLite * instance
Definition: ll40ls.cpp:65
int orb_exists(const struct orb_metadata *meta, int instance)
Definition: uORB.cpp:105
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)
Definition: HealthFlags.cpp:44
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
static bool magnometerCheck(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.
Definition: drv_hrt.h:102
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
Contains helper functions to efficiently set the system health flags from commander and preflight che...