mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-29 13:50:35 +08:00
Julian Oes
648a21f11d
The calibration is not found in HITL mode. Therefore, I suggest to ignore this step and assume the calibration is fine. This mostly fixes the preflight check indicator in QGC, arming was (for some reason?) already possible.
105 lines
3.6 KiB
C++
105 lines
3.6 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "../PreFlightCheck.hpp"
|
|
|
|
#include <drivers/drv_hrt.h>
|
|
#include <HealthFlags.h>
|
|
#include <math.h>
|
|
#include <px4_defines.h>
|
|
#include <lib/sensor_calibration/Utilities.hpp>
|
|
#include <lib/systemlib/mavlink_log.h>
|
|
#include <uORB/Subscription.hpp>
|
|
#include <uORB/topics/sensor_accel.h>
|
|
|
|
using namespace time_literals;
|
|
|
|
bool PreFlightCheck::accelerometerCheck(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)
|
|
{
|
|
const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
|
|
bool calibration_valid = false;
|
|
bool valid = true;
|
|
|
|
if (exists) {
|
|
|
|
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
|
|
|
|
valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
|
|
|
|
if (!valid) {
|
|
if (report_fail) {
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
|
|
}
|
|
}
|
|
|
|
device_id = accel.get().device_id;
|
|
|
|
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
|
calibration_valid = true;
|
|
|
|
} else {
|
|
calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
|
|
}
|
|
|
|
if (!calibration_valid) {
|
|
if (report_fail) {
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
|
|
}
|
|
|
|
} else {
|
|
const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
|
|
+ accel.get().y * accel.get().y
|
|
+ accel.get().z * accel.get().z);
|
|
|
|
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
|
if (report_fail) {
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
|
|
}
|
|
|
|
// this is fatal
|
|
valid = false;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
if (!optional && report_fail) {
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
|
|
}
|
|
}
|
|
|
|
const bool success = calibration_valid && valid;
|
|
|
|
return success;
|
|
}
|