PX4 Firmware
PX4 Autopilot Software http://px4.io
accelerometerCheck.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 <math.h>
39 #include <px4_defines.h>
40 #include <systemlib/mavlink_log.h>
41 #include <uORB/Subscription.hpp>
44 
45 using namespace time_literals;
46 
48  const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
49 {
50  const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
51  bool calibration_valid = false;
52  bool accel_valid = true;
53 
54  if (exists) {
55 
56  uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
57 
58  accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
59 
60  if (!accel_valid) {
61  if (report_fail) {
62  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance);
63  }
64  }
65 
66  device_id = accel.get().device_id;
67 
68  calibration_valid = check_calibration("CAL_ACC%u_ID", device_id);
69 
70  if (!calibration_valid) {
71  if (report_fail) {
72  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance);
73  }
74 
75  } else {
76 
77  if (dynamic) {
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);
81 
82  if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
83  if (report_fail) {
84  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
85  }
86 
87  /* this is frickin' fatal */
88  accel_valid = false;
89  }
90  }
91  }
92 
93  } else {
94  if (!optional && report_fail) {
95  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor #%u missing", instance);
96  }
97  }
98 
99  const bool success = calibration_valid && accel_valid;
100 
101  if (instance == 0) {
102  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status);
103 
104  } else if (instance == 1) {
105  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status);
106  }
107 
108  return success;
109 }
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
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
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
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...