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

Publish the satcom subsystem health

This commit is contained in:
acfloria 2018-07-05 10:05:40 +02:00 committed by Lorenz Meier
parent 1daaa6f5dc
commit 21ebcdc0ae
3 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/subsystem_info.msg
View File

@ -25,7 +25,7 @@ uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
uint64 SUBSYSTEM_TYPE_LOGGING = 16777216 uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432 uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864 uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
uint64 SUBSYSTEM_TYPE_MISSION = 134217728 uint64 SUBSYSTEM_TYPE_SATCOM = 134217728
bool present bool present
bool enabled bool enabled

24
src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
View File

@ -300,6 +300,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
break; break;
} }
publish_subsystem_status();
if (_new_state != _state) { if (_new_state != _state) {
VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]); VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
_state = _new_state; _state = _new_state;
@ -1061,6 +1062,29 @@ void IridiumSBD::publish_iridium_status()
} }
void IridiumSBD::publish_subsystem_status()
{
const bool present = true;
const bool enabled = true;
const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made
if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) {
_info.timestamp = hrt_absolute_time();
_info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM;
_info.present = present;
_info.enabled = enabled;
_info.ok = ok;
if (_subsystem_pub == nullptr) {
_subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info);
}
}
}
int IridiumSBD::open_first(struct file *filep) int IridiumSBD::open_first(struct file *filep)
{ {
_cdev_used = true; _cdev_used = true;

5
src/drivers/telemetry/iridiumsbd/IridiumSBD.h
View File

@ -41,6 +41,7 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/iridiumsbd_status.h> #include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/subsystem_info.h>
typedef enum { typedef enum {
SATCOM_OK = 0, SATCOM_OK = 0,
@ -256,6 +257,8 @@ private:
void publish_iridium_status(void); void publish_iridium_status(void);
void publish_subsystem_status();
/** /**
* Notification of the first open of CDev. * Notification of the first open of CDev.
* *
@ -301,6 +304,7 @@ private:
uint16_t _packet_length = 0; uint16_t _packet_length = 0;
orb_advert_t _iridiumsbd_status_pub = nullptr; orb_advert_t _iridiumsbd_status_pub = nullptr;
orb_advert_t _subsystem_pub = nullptr;
bool _test_pending = false; bool _test_pending = false;
char _test_command[32]; char _test_command[32];
@ -338,4 +342,5 @@ private:
bool _verbose = false; bool _verbose = false;
iridiumsbd_status_s _status = {}; iridiumsbd_status_s _status = {};
subsystem_info_s _info = {};
}; };