dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Subsystem_info status flags & checks: Switch back to uORB for inter-process communication, handle GPS checks completely inside ekf2, add distance_sensor checks

This commit is contained in:
Philipp Oettershagen 2018-05-25 18:05:02 +02:00 committed by Beat Küng
parent 6f1f414b49
commit f5847a4a7b
13 changed files with 109 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/subsystem_info.msg
View File

@ -31,3 +31,5 @@ bool present
bool enabled
bool ok
uint64 subsystem_type
uint32 ORB_QUEUE_LENGTH = 25

6
src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp
View File

@ -58,14 +58,12 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#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

5
src/drivers/distance_sensor/mb12xx/mb12xx.cpp
View File

@ -61,14 +61,12 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@ -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

2
src/drivers/distance_sensor/sf0x/sf0x.cpp
View File

@ -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

5
src/drivers/distance_sensor/sf1xx/sf1xx.cpp
View File

@ -64,14 +64,12 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@ -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

5
src/drivers/distance_sensor/srf02/srf02.cpp
View File

@ -60,14 +60,12 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@ -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

5
src/drivers/distance_sensor/teraranger/teraranger.cpp
View File

@ -60,14 +60,12 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@ -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

5
src/drivers/distance_sensor/tfmini/tfmini.cpp
View File

@ -61,7 +61,6 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -69,7 +68,6 @@
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@ -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

5
src/drivers/px4flow/px4flow.cpp
View File

@ -65,7 +65,6 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <systemlib/subsystem_info_pub.h>
#include <conversion/rotation.h>
@ -75,7 +74,6 @@
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
@ -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

69
src/modules/commander/PreflightCheck.cpp
View File

@ -71,6 +71,7 @@
#include <uORB/topics/system_power.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#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;

40
src/modules/commander/commander.cpp
View File

@ -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);
}
}

79
src/modules/systemlib/subsystem_info_pub.cpp
View File

@ -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 <uORB/topics/subsystem_info.h>
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;
}

5
src/modules/systemlib/subsystem_info_pub.h
View File

@ -45,7 +45,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
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);