fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 17:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/subsystem_info_pub.cpp
+30 -49
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;
}
src/modules/systemlib/subsystem_info_pub.h
+1 -4
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);