mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 19:32:19 +08:00
mavlink doesn't have already published message
- if the land detector started before the mavlink module it won't have a valid message vehicle_land_detected message until published again
This commit is contained in:
@@ -39,13 +39,15 @@
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "mavlink_orb_subscription.h"
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "mavlink_orb_subscription.h"
|
||||
#include <px4_defines.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
|
||||
next(nullptr),
|
||||
@@ -195,6 +197,14 @@ MavlinkOrbSubscription::is_published()
|
||||
_published = true;
|
||||
}
|
||||
|
||||
// topic may have been last published before we subscribed
|
||||
uint64_t time_topic = 0;
|
||||
if (!_published && orb_stat(_fd, &time_topic) == PX4_OK) {
|
||||
if (time_topic != 0) {
|
||||
_published = true;
|
||||
}
|
||||
}
|
||||
|
||||
return _published;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user