mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 23:13:33 +08:00
Subsystem_info status flags & checks : Initial commit, updating the health flags in a centralized way mostly in commander and the votedSensorsUpdate function.
This commit is contained in:
Philipp Oettershagen
committed by
Beat Küng
Beat Küng
parent
40e6a5a39b
commit
6f1f414b49
@@ -55,6 +55,7 @@
|
||||
#include <parameters/param.h>
|
||||
#include <systemlib/rc_check.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <systemlib/subsystem_info_pub.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
@@ -69,6 +70,7 @@
|
||||
#include <uORB/topics/sensor_preflight.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
#include "PreflightCheck.h"
|
||||
|
||||
@@ -118,7 +120,9 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
|
||||
|
||||
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
|
||||
{
|
||||
bool present = true;
|
||||
bool success = true;
|
||||
int ret = 0;
|
||||
|
||||
char s[30];
|
||||
sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
|
||||
@@ -131,11 +135,12 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
present = false;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
|
||||
ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
|
||||
|
||||
if (ret) {
|
||||
if (report_fail) {
|
||||
@@ -158,6 +163,9 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
|
||||
}
|
||||
|
||||
out:
|
||||
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success);
|
||||
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success);
|
||||
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
}
|
||||
@@ -182,8 +190,9 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
|
||||
if (sensors.accel_inconsistency_m_s_s > test_limit) {
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false);
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false);
|
||||
}
|
||||
|
||||
success = false;
|
||||
goto out;
|
||||
|
||||
@@ -199,6 +208,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
|
||||
if (sensors.gyro_inconsistency_rad_s > test_limit) {
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false);
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false);
|
||||
}
|
||||
|
||||
success = false;
|
||||
@@ -238,7 +249,8 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
|
||||
}
|
||||
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false);
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false);
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -247,7 +259,9 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
|
||||
|
||||
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
|
||||
{
|
||||
bool present = true;
|
||||
bool success = true;
|
||||
int ret = 0;
|
||||
|
||||
char s[30];
|
||||
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
|
||||
@@ -260,11 +274,12 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
present = false;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
|
||||
ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
|
||||
|
||||
if (ret) {
|
||||
if (report_fail) {
|
||||
@@ -321,13 +336,18 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
|
||||
#endif
|
||||
|
||||
out:
|
||||
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success);
|
||||
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success);
|
||||
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
}
|
||||
|
||||
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
|
||||
{
|
||||
bool present = true;
|
||||
bool success = true;
|
||||
int ret = 0;
|
||||
|
||||
char s[30];
|
||||
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
|
||||
@@ -340,11 +360,12 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
present = false;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
|
||||
ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
|
||||
|
||||
if (ret) {
|
||||
if (report_fail) {
|
||||
@@ -367,6 +388,9 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
|
||||
}
|
||||
|
||||
out:
|
||||
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success);
|
||||
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success);
|
||||
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
}
|
||||
@@ -386,7 +410,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
|
||||
}
|
||||
}
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, !optional, false);
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -402,13 +426,14 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
|
||||
// }
|
||||
|
||||
//out:
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success);
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
}
|
||||
|
||||
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm)
|
||||
{
|
||||
bool present = true;
|
||||
bool success = true;
|
||||
|
||||
int fd_airspeed = orb_subscribe(ORB_ID(airspeed));
|
||||
@@ -419,20 +444,20 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
|
||||
|
||||
if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) ||
|
||||
(hrt_elapsed_time(&differential_pressure.timestamp) > 1000000)) {
|
||||
if (report_fail) {
|
||||
if (report_fail && !optional) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
|
||||
}
|
||||
|
||||
present = false;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) ||
|
||||
(hrt_elapsed_time(&airspeed.timestamp) > 1000000)) {
|
||||
if (report_fail) {
|
||||
if (report_fail && !optional) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
|
||||
}
|
||||
|
||||
present = false;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
@@ -447,7 +472,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK");
|
||||
}
|
||||
|
||||
present = true;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
@@ -461,12 +486,13 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
|
||||
}
|
||||
|
||||
present = true;
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
out:
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success);
|
||||
orb_unsubscribe(fd_airspeed);
|
||||
orb_unsubscribe(fd_diffpres);
|
||||
return success;
|
||||
@@ -523,6 +549,7 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
|
||||
static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required)
|
||||
{
|
||||
bool success = true; // start with a pass and change to a fail if any test fails
|
||||
bool present = true;
|
||||
float test_limit = 1.0f; // pass limit re-used for each test
|
||||
|
||||
// Get estimator status data if available and exit with a fail recorded if not
|
||||
@@ -530,6 +557,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
|
||||
estimator_status_s status = {};
|
||||
|
||||
if (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK) {
|
||||
present = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
@@ -626,12 +654,13 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
|
||||
// The EKF is not using GPS
|
||||
if (report_fail) {
|
||||
if (ekf_gps_check_fail) {
|
||||
// Poor GPS qulaity is the likely cause
|
||||
// Poor GPS quality is the likely cause
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
|
||||
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
|
||||
} else {
|
||||
// Likely cause unknown
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -649,6 +678,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
|
||||
if (gps_quality_fail) {
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
|
||||
}
|
||||
|
||||
success = false;
|
||||
@@ -658,15 +688,15 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
|
||||
}
|
||||
|
||||
out:
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present);
|
||||
orb_unsubscribe(sub);
|
||||
return success;
|
||||
}
|
||||
|
||||
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
|
||||
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
|
||||
vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
|
||||
const hrt_abstime &time_since_boot)
|
||||
{
|
||||
|
||||
if (time_since_boot < 2000000) {
|
||||
// the airspeed driver filter doesn't deliver the actual value yet
|
||||
reportFailures = false;
|
||||
@@ -737,7 +767,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
if ((reportFailures && !failed)) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
|
||||
}
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false);
|
||||
failed = true;
|
||||
}
|
||||
|
||||
@@ -775,7 +805,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
if ((reportFailures && !failed)) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
|
||||
}
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
@@ -808,7 +838,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
if ((reportFailures && !failed)) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
|
||||
}
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
@@ -844,7 +874,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
if (reportFailures && !failed) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
|
||||
}
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
@@ -858,7 +888,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
|
||||
/* ---- AIRSPEED ---- */
|
||||
if (checkAirspeed) {
|
||||
if (!airspeedCheck(mavlink_log_pub, true, reportFailures && !failed, prearm)) {
|
||||
int32_t optional = 0; //Given checkAirspeed==true, assume the airspeed sensor is also required by default
|
||||
param_get(param_find("FW_ARSP_MODE"), &optional);
|
||||
|
||||
if (!airspeedCheck(mavlink_log_pub, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
@@ -871,6 +904,13 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
}
|
||||
|
||||
failed = true;
|
||||
|
||||
publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, false);
|
||||
status_flags.rc_calibration_valid = false;
|
||||
} else {
|
||||
// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
|
||||
status_flags.rc_calibration_valid = true;
|
||||
publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, !status.rc_signal_lost);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -881,6 +921,29 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- GPS ---- */
|
||||
if (checkGNSS) {
|
||||
int fd_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
vehicle_gps_position_s gps = {};
|
||||
bool present = true;
|
||||
|
||||
if ((orb_copy(ORB_ID(vehicle_gps_position), fd_gps, &gps) != PX4_OK) ||
|
||||
(hrt_elapsed_time(&gps.timestamp) > 2000000)) {
|
||||
if (reportFailures) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS MODULE MISSING");
|
||||
}
|
||||
present = false;
|
||||
}
|
||||
|
||||
// Mark GPS as required (given that checkGNSS=true) and indicate whether it is present. For now also assume it is healthy
|
||||
// if there is a lock ... EKF2 will then set the healthy=false if its more extensive GPS checks fail in the next step.
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, present, true, present && gps.fix_type>=3);
|
||||
}
|
||||
|
||||
// TODO: Add rangefinder here. We have the SENS_EN_XXX params that tell us what we should have. This is currently completely done inside the driver.
|
||||
|
||||
// TODO: Add optical flow check here? This is currently completely done inside the driver.
|
||||
|
||||
/* ---- Navigation EKF ---- */
|
||||
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
|
||||
int32_t estimator_type;
|
||||
@@ -889,8 +952,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
|
||||
if (estimator_type == 2) {
|
||||
// don't report ekf failures for the first 10 seconds to allow time for the filter to start
|
||||
bool report_ekf_fail = (time_since_boot > 10 * 1000000);
|
||||
|
||||
if (!ekf2Check(mavlink_log_pub, true, (reportFailures && report_ekf_fail && !failed), checkGNSS)) {
|
||||
if (!ekf2Check(mavlink_log_pub, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -43,6 +43,8 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace Preflight
|
||||
@@ -73,7 +75,7 @@ namespace Preflight
|
||||
* true if the system power should be checked
|
||||
**/
|
||||
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
|
||||
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
|
||||
vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
|
||||
const hrt_abstime &time_since_boot);
|
||||
|
||||
static constexpr unsigned max_mandatory_gyro_count = 1;
|
||||
|
||||
@@ -79,6 +79,7 @@
|
||||
#include <parameters/param.h>
|
||||
#include <systemlib/rc_check.h>
|
||||
#include <systemlib/state_table.h>
|
||||
#include <systemlib/subsystem_info_pub.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cfloat>
|
||||
@@ -1256,6 +1257,7 @@ Commander::run()
|
||||
|
||||
status_flags.condition_power_input_valid = true;
|
||||
status_flags.usb_connected = false;
|
||||
status_flags.rc_calibration_valid = true;
|
||||
|
||||
// CIRCUIT BREAKERS
|
||||
status_flags.circuit_breaker_engaged_power_check = false;
|
||||
@@ -1270,6 +1272,11 @@ Commander::run()
|
||||
status_flags.condition_local_velocity_valid = false;
|
||||
status_flags.condition_local_altitude_valid = false;
|
||||
|
||||
bool status_changed = true;
|
||||
|
||||
/* initialize the vehicle status flag helper functions. This also initializes the sensor health flags*/
|
||||
publish_subsystem_info_init(&status, &status_changed);
|
||||
|
||||
/* publish initial state */
|
||||
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
|
||||
|
||||
@@ -1310,7 +1317,6 @@ Commander::run()
|
||||
bool emergency_battery_voltage_actions_done = false;
|
||||
bool dangerous_battery_level_requests_poweroff = false;
|
||||
|
||||
bool status_changed = true;
|
||||
bool param_init_forced = true;
|
||||
|
||||
bool updated = false;
|
||||
@@ -1825,6 +1831,12 @@ Commander::run()
|
||||
}
|
||||
}
|
||||
|
||||
if((_last_condition_global_position_valid != status_flags.condition_global_position_valid) && status_flags.condition_global_position_valid) {
|
||||
// If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa.
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true);
|
||||
publish_subsystem_info_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true);
|
||||
}
|
||||
|
||||
check_valid(_local_position_sub.get().timestamp, _failsafe_pos_delay.get() * 1_s, _local_position_sub.get().z_valid, &(status_flags.condition_local_altitude_valid), &status_changed);
|
||||
|
||||
/* Update land detector */
|
||||
@@ -1993,41 +2005,6 @@ Commander::run()
|
||||
}
|
||||
}
|
||||
|
||||
/* update subsystem */
|
||||
orb_check(subsys_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
|
||||
|
||||
//warnx("subsystem changed: %d\n", (int)info.subsystem_type);
|
||||
|
||||
/* mark / unmark as present */
|
||||
if (info.present) {
|
||||
status.onboard_control_sensors_present |= info.subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_present &= ~info.subsystem_type;
|
||||
}
|
||||
|
||||
/* mark / unmark as enabled */
|
||||
if (info.enabled) {
|
||||
status.onboard_control_sensors_enabled |= info.subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_enabled &= ~info.subsystem_type;
|
||||
}
|
||||
|
||||
/* mark / unmark as ok */
|
||||
if (info.ok) {
|
||||
status.onboard_control_sensors_health |= info.subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_health &= ~info.subsystem_type;
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
|
||||
|
||||
@@ -2223,11 +2200,13 @@ Commander::run()
|
||||
/* handle the case where RC signal was regained */
|
||||
if (!status_flags.rc_signal_found_once) {
|
||||
status_flags.rc_signal_found_once = true;
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid);
|
||||
status_changed = true;
|
||||
|
||||
} else {
|
||||
if (status.rc_signal_lost) {
|
||||
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", hrt_elapsed_time(&rc_signal_lost_timestamp) / 1000);
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid);
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
@@ -2375,6 +2354,7 @@ Commander::run()
|
||||
mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
|
||||
status.rc_signal_lost = true;
|
||||
rc_signal_lost_timestamp = sp_man.timestamp;
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false);
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
@@ -4096,6 +4076,9 @@ void Commander::poll_telemetry_status()
|
||||
(mission_result.instance_count > 0) && !mission_result.valid) {
|
||||
|
||||
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
|
||||
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false); // TODO
|
||||
} else {
|
||||
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true); // TODO
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user