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

add subscriber handler and status displays

Browse Source
This commit is contained in:
ChristophTobler
2017-10-23 10:32:26 +02:00
committed by Beat Küng
parent ba11e0dc44
commit 9be8d6acc9
8 changed files with 456 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/events/subscriber_handler.cpp
+112
View File
@@ -0,0 +1,112 @@
#include "subscriber_handler.h"
#include <px4_log.h>
using namespace events;
void SubscriberHandler::subscribe()
{
if (_battery_status_sub < 0) {
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
}
if (_cpuload_sub < 0) {
_cpuload_sub = orb_subscribe(ORB_ID(cpuload));
}
if (_vehicle_command_sub < 0) {
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
if (_vehicle_status_flags_sub < 0) {
_vehicle_status_flags_sub = orb_subscribe(ORB_ID(vehicle_status_flags));
}
if (_vehicle_attitude_sub < 0) {
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
}
}
void SubscriberHandler::unsubscribe()
{
if (_battery_status_sub >= 0) {
orb_unsubscribe(_battery_status_sub);
_battery_status_sub = -1;
}
if (_cpuload_sub >= 0) {
orb_unsubscribe(_cpuload_sub);
_cpuload_sub = -1;
}
if (_vehicle_command_sub >= 0) {
orb_unsubscribe(_vehicle_command_sub);
_vehicle_command_sub = -1;
}
if (_vehicle_status_sub >= 0) {
orb_unsubscribe(_vehicle_status_sub);
_vehicle_status_sub = -1;
}
if (_vehicle_status_flags_sub >= 0) {
orb_unsubscribe(_vehicle_status_flags_sub);
_vehicle_status_flags_sub = -1;
}
if (_vehicle_attitude_sub >= 0) {
orb_unsubscribe(_vehicle_attitude_sub);
_vehicle_attitude_sub = -1;
}
}
void SubscriberHandler::check_for_updates()
{
bool updated;
_update_bitfield = 0;
orb_check(_vehicle_command_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleCommand;
}
updated = false;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleStatus;
}
updated = false;
orb_check(_vehicle_status_flags_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleStatusFlags;
}
updated = false;
orb_check(_battery_status_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::BatteryStatus;
}
updated = false;
orb_check(_cpuload_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::CpuLoad;
}
updated = false;
orb_check(_vehicle_attitude_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleAttitude;
}
}