fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: PreFlightCheck remove HIL special cases

Browse Source 
- skipping these sensor checks in HIL no longer makes sense as each
sensor has a normal publication (sensor_accel/gyro/etc) regardless of
simulation
This commit is contained in:
Daniel Agar 2020-10-22 11:51:03 -04:00
parent 4378254182
commit 81765bc06a
3 changed files with 21 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -59,29 +59,15 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
const hrt_abstime &time_since_boot)
{
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
const bool checkSensors = !hil_enabled;
const bool checkDynamic = !hil_enabled;
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status_flags.circuit_breaker_engaged_airspd_check &&
(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
checkAirspeed = true;
}
bool failed = false;
failed = failed || !airframeCheck(mavlink_log_pub, status);
/* ---- MAG ---- */
if (checkSensors) {
{
int32_t sys_has_mag = 1;
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
@ -111,7 +97,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
/* ---- ACCEL ---- */
if (checkSensors) {
{
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
const bool required = (i < max_mandatory_accel_count);
@ -119,7 +105,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
@ -130,7 +116,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
/* ---- GYRO ---- */
if (checkSensors) {
{
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
const bool required = (i < max_mandatory_gyro_count);
@ -147,7 +133,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
/* ---- BARO ---- */
if (checkSensors) {
{
int32_t sys_has_baro = 1;
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
@ -170,14 +156,17 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- IMU CONSISTENCY ---- */
// To be performed after the individual sensor checks have completed
if (checkSensors) {
{
if (!imuConsistencyCheck(mavlink_log_pub, status, reportFailures)) {
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
/* Perform airspeed check only if circuit breaker is not engaged and it's not a rotary wing */
if (!status_flags.circuit_breaker_engaged_airspd_check &&
(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
int32_t optional = 0;
param_get(param_find("FW_ARSP_MODE"), &optional);

2
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
View File

@ -97,7 +97,7 @@ private:
const bool optional, int32_t &device_id, const bool report_fail);
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
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);
const bool optional, int32_t &device_id, const bool report_fail);
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 bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,

23
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
View File

@ -46,7 +46,7 @@
using namespace time_literals;
bool PreFlightCheck::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)
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;
@ -74,20 +74,17 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s
}
} 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 (dynamic) {
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 frickin' fatal */
valid = false;
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;
}
}