diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index 5784a3f589..1392b77e42 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -31,3 +31,5 @@ bool present bool enabled bool ok uint64 subsystem_type + +uint32 ORB_QUEUE_LENGTH = 25 diff --git a/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp b/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp index cda6cc328d..56559bf0e9 100644 --- a/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp +++ b/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp @@ -58,14 +58,12 @@ #include #include -#include #include #include #include #include -#include #include #define SR04_MAX_RANGEFINDERS 6 @@ -623,10 +621,6 @@ HC_SR04::start() (worker_t)&HC_SR04::cycle_trampoline, this, USEC2TICK(_cycling_rate)); - - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index 6d6b0a9dd5..977efaa4cd 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -61,14 +61,12 @@ #include #include -#include #include #include #include #include -#include #include #include @@ -589,9 +587,6 @@ MB12XX::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index 685d9aa001..9369a2a82a 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -643,8 +643,6 @@ SF0X::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); - // /* notify about state change */ - // publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 480c32c511..1c1fd1eeac 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -64,14 +64,12 @@ #include #include -#include #include #include #include #include -#include #include #include @@ -586,9 +584,6 @@ SF1XX::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval)); - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index 55dd2773f7..6acd389cb1 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -60,14 +60,12 @@ #include #include -#include #include #include #include #include -#include #include #include @@ -592,9 +590,6 @@ SRF02::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5); - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index 0343236f53..438fc80ba1 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -60,14 +60,12 @@ #include #include -#include #include #include #include #include -#include #include #include @@ -669,9 +667,6 @@ TERARANGER::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&TERARANGER::cycle_trampoline, this, 1); - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index c595dffeb2..25bb659d6d 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -61,7 +61,6 @@ #include #include -#include #include #include @@ -69,7 +68,6 @@ #include #include -#include #include #include @@ -608,9 +606,6 @@ TFMINI::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, 1); - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true); } void diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f38af3739a..beeba53d45 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -65,7 +65,6 @@ #include #include #include -#include #include @@ -75,7 +74,6 @@ #include #include -#include #include #include @@ -588,9 +586,6 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - - /* notify about state change */ - publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW, true, true, true); } void diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 7168181dec..33b9b7e6da 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include "PreflightCheck.h" @@ -552,6 +553,9 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ bool present = true; float test_limit = 1.0f; // pass limit re-used for each test + bool gps_success = true; + bool gps_present = true; + // Get estimator status data if available and exit with a fail recorded if not int sub = orb_subscribe(ORB_ID(estimator_status)); estimator_status_s status = {}; @@ -652,16 +656,15 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ if (!ekf_gps_fusion) { // The EKF is not using GPS - if (report_fail) { - if (ekf_gps_check_fail) { - // 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); - } + if (ekf_gps_check_fail) { + // Poor GPS quality is the likely cause + if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + gps_success = false; + } else { + // Likely cause unknown + if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); + gps_success = false; + gps_present = false; } success = false; @@ -676,11 +679,8 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0); 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); - } - + if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + gps_success = false; success = false; goto out; } @@ -689,6 +689,7 @@ 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); + publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success); orb_unsubscribe(sub); return success; } @@ -709,6 +710,20 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu const bool checkDynamic = !hil_enabled; const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check); + bool checkDistanceSensors = false; + int32_t distSensorEnabled[7]; + param_get(param_find("SENS_EN_LEDDAR1"), &(distSensorEnabled[0])); + param_get(param_find("SENS_EN_LL40LS"), &distSensorEnabled[1]); + param_get(param_find("SENS_EN_MB12XX"), &distSensorEnabled[2]); + param_get(param_find("SENS_EN_SF0X"), &distSensorEnabled[3]); + param_get(param_find("SENS_EN_SF1XX"), &distSensorEnabled[4]); + param_get(param_find("SENS_EN_TFMINI"), &distSensorEnabled[5]); + param_get(param_find("SENS_EN_TRANGER"), &distSensorEnabled[6]); + if(distSensorEnabled[0]>0 || distSensorEnabled[1]>0 || distSensorEnabled[2]>0 || distSensorEnabled[3]>0 || + distSensorEnabled[4]>0 || distSensorEnabled[5]>0 || distSensorEnabled[6]>0) { + checkDistanceSensors=true; + } + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not @@ -921,29 +936,23 @@ 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 = {}; + /* ---- DISTANCE SENSORS ---- */ + if (checkDistanceSensors && time_since_boot > 10 * 1000000) { + int fd_distance_sensor = orb_subscribe(ORB_ID(distance_sensor)); + distance_sensor_s dist_sensor = {}; bool present = true; - if ((orb_copy(ORB_ID(vehicle_gps_position), fd_gps, &gps) != PX4_OK) || - (hrt_elapsed_time(&gps.timestamp) > 2000000)) { + if ((orb_copy(ORB_ID(distance_sensor), fd_distance_sensor, &dist_sensor) != PX4_OK) || + (hrt_elapsed_time(&dist_sensor.timestamp) > 2000000)) { if (reportFailures) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS MODULE MISSING"); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: DISTANCE SENSOR 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); + publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, present, true, present); + orb_unsubscribe(fd_distance_sensor); } - // 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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 42b033fa6b..66b3e8dd22 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1274,9 +1274,6 @@ Commander::run() 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); @@ -2005,6 +2002,42 @@ Commander::run() } } + /* update subsystem */ + do { + 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; + } + } while(updated); + /* 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) { @@ -2392,6 +2425,7 @@ Commander::run() status_changed = true; PX4_ERR("Engine Failure"); + publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL,true,true,false); } } diff --git a/src/modules/systemlib/subsystem_info_pub.cpp b/src/modules/systemlib/subsystem_info_pub.cpp index 8b12ab64de..94c81a27d0 100644 --- a/src/modules/systemlib/subsystem_info_pub.cpp +++ b/src/modules/systemlib/subsystem_info_pub.cpp @@ -34,65 +34,59 @@ /** * @file subsystem_info_pub.cpp * - * Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code. It is basically a - * helper function for commander. Approach: - * - Before commander starts (which happens after some of the drivers have already published the respective subsystem_info), this helper - * code stores all requests for a publish_subsystem_info in the internal_status variable - * - When commander starts up, it calls the publish_subsystem_info_init function. This 1) copies the internal_status into commander's - * vehicle status variable and 2) assigns the status pointer to commanders vehicle status - * - After that, all requests to publish_subsystem_info are directly written to commander's vehicle status such that it is always up - * to date. Commander then publishes the vehicle_status uORB (and is in fact the only app that does that, which is why this approach works) + * Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code. * * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch) */ #include "subsystem_info_pub.h" +#include -vehicle_status_s internal_status = {}; -vehicle_status_s *status = &internal_status; -bool *status_changed = nullptr; +static orb_advert_t pub = nullptr; +struct subsystem_info_s info = {}; +struct vehicle_status_s status; -/* initialize pointer to commander's vehicle status variable */ -void publish_subsystem_info_init(vehicle_status_s *commander_vehicle_status_ptr, bool *commander_status_changed_ptr) -{ - status = commander_vehicle_status_ptr; - status_changed = commander_status_changed_ptr; - status->onboard_control_sensors_present = internal_status.onboard_control_sensors_present; - status->onboard_control_sensors_enabled = internal_status.onboard_control_sensors_enabled; - status->onboard_control_sensors_health = internal_status.onboard_control_sensors_health; - *status_changed = true; -} - -/* Writes the full state information for a specific subsystem type, either directly into commander's vehicle - * status variable or into an internal variable that is later copied to commander's vehicle status variable*/ void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok) { - PX4_DEBUG("publish_subsystem_info (ext:%u): Type %llu pres=%u enabl=%u ok=%u", status != &internal_status, - subsystem_type, present, enabled, ok); + PX4_INFO("publish_subsystem_info: Type %llu pres=%u enabl=%u ok=%u", subsystem_type, present, enabled, ok); + // Keep a local copy of the status flags. Because we use queuing, it could be that the flags in the vehicle_status topics are + // not up to date. When using those publish_subsystem_info_xxx functions that only write a subset of health flags but leave others + // unchanged, we'd write outdated health flags to vehicle_status. Having an up to date local copy resolves that issue. if (present) { - status->onboard_control_sensors_present |= (uint32_t)subsystem_type; + status.onboard_control_sensors_present |= (uint32_t)subsystem_type; } else { - status->onboard_control_sensors_present &= ~(uint32_t)subsystem_type; + status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type; } if (enabled) { - status->onboard_control_sensors_enabled |= (uint32_t)subsystem_type; + status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type; } else { - status->onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type; + status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type; } if (ok) { - status->onboard_control_sensors_health |= (uint32_t)subsystem_type; + status.onboard_control_sensors_health |= (uint32_t)subsystem_type; } else { - status->onboard_control_sensors_health &= ~(uint32_t)subsystem_type; + status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type; } - if (status != &internal_status) { *status_changed = true; } + /* Publish the subsystem_info message */ + info.present = present; + info.enabled = enabled; + info.ok = ok; + info.subsystem_type = subsystem_type; + + if (pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise_queue(ORB_ID(subsystem_info), &info, subsystem_info_s::ORB_QUEUE_LENGTH); + } } void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy) @@ -120,29 +114,16 @@ void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok) publish_subsystem_info(subsystem_type, getPresent(subsystem_type), getEnabled(subsystem_type), ok); } -void publish_subsystem_info_print() -{ - uint64_t type = 1; - - for (int i = 1; i < 31; i++) { - PX4_DEBUG("subsystem_info: Type %llu pres=%u enabl=%u ok=%u", type, - (status->onboard_control_sensors_present & (uint32_t)type) > 0, - (status->onboard_control_sensors_enabled & (uint32_t)type) > 0, - (status->onboard_control_sensors_health & (uint32_t)type) > 0); - type = type * 2; - } -} - // Local helper functions bool getPresent(uint64_t subsystem_type) { - return status->onboard_control_sensors_present & (uint32_t)subsystem_type; + return status.onboard_control_sensors_present & (uint32_t)subsystem_type; } bool getEnabled(uint64_t subsystem_type) { - return status->onboard_control_sensors_enabled & (uint32_t)subsystem_type; + return status.onboard_control_sensors_enabled & (uint32_t)subsystem_type; } bool getHealthy(uint64_t subsystem_type) { - return status->onboard_control_sensors_health & (uint32_t)subsystem_type; + return status.onboard_control_sensors_health & (uint32_t)subsystem_type; } diff --git a/src/modules/systemlib/subsystem_info_pub.h b/src/modules/systemlib/subsystem_info_pub.h index 155eae8f70..29f263ccd7 100644 --- a/src/modules/systemlib/subsystem_info_pub.h +++ b/src/modules/systemlib/subsystem_info_pub.h @@ -45,7 +45,6 @@ #include #include -void publish_subsystem_info_init(vehicle_status_s *commander_vehicle_status_ptr, bool *commander_status_changed_ptr); void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok); void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy); @@ -54,9 +53,7 @@ void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enable void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled); void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok); -void publish_subsystem_info_print(); - -// Local helper functions +//// Local helper functions bool getPresent(uint64_t subsystem_type); bool getEnabled(uint64_t subsystem_type); bool getHealthy(uint64_t subsystem_type);