fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-11 03:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: multi-EKF replay hacks and replay overhaul

Browse Source
This commit is contained in:
Daniel Agar
2021-08-18 22:16:04 -04:00
parent 1af2ecaff6
commit af8d1c9c6c
52 changed files with 773 additions and 1294 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/px4_work_queue/WorkQueue.cpp
+5
View File
@@ -111,6 +111,7 @@ void WorkQueue::Add(WorkItem *item)
if (_lockstep_component == -1) {
_lockstep_component = px4_lockstep_register_component();
//fprintf(stderr, "WQ %s px4_lockstep_register_component:%d\n", item->ItemName(), _lockstep_component);
}
#endif // ENABLE_LOCKSTEP_SCHEDULER
@@ -170,6 +171,10 @@ void WorkQueue::Run()
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (_q.empty()) {
if (_lockstep_component > 0) {
//fprintf(stderr, "WQ px4_lockstep_unregister_component:%d\n", _lockstep_component);
}
px4_lockstep_unregister_component(_lockstep_component);
_lockstep_component = -1;
}
platforms/common/uORB/uORBManager.cpp
-237
View File
@@ -66,29 +66,6 @@ bool uORB::Manager::terminate()
return false;
}
uORB::Manager::Manager()
{
#ifdef ORB_USE_PUBLISHER_RULES
const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules";
int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
if (ret == PX4_OK) {
_has_publisher_rules = true;
PX4_INFO("Using orb rules from %s", file_name);
} else {
PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
}
#endif /* ORB_USE_PUBLISHER_RULES */
}
uORB::Manager::~Manager()
{
delete _device_master;
}
uORB::DeviceMaster *uORB::Manager::get_device_master()
{
if (!_device_master) {
@@ -170,30 +147,6 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
unsigned int queue_size)
{
#ifdef ORB_USE_PUBLISHER_RULES
// check publisher rule
if (_has_publisher_rules) {
const char *prog_name = px4_get_taskname();
if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
if (_publisher_rule.ignore_other_topics) {
if (!findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
} else {
if (findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
}
#endif /* ORB_USE_PUBLISHER_RULES */
/* open the node as an advertiser */
int fd = node_open(meta, true, instance);
@@ -242,14 +195,6 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
int uORB::Manager::orb_unadvertise(orb_advert_t handle)
{
#ifdef ORB_USE_PUBLISHER_RULES
if (handle == _Instance) {
return PX4_OK; //pretend success
}
#endif /* ORB_USE_PUBLISHER_RULES */
return uORB::DeviceNode::unadvertise(handle);
}
@@ -264,54 +209,11 @@ int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned
return node_open(meta, false, &inst);
}
int uORB::Manager::orb_unsubscribe(int fd)
{
return px4_close(fd);
}
int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
{
#ifdef ORB_USE_PUBLISHER_RULES
if (handle == _Instance) {
return PX4_OK; //pretend success
}
#endif /* ORB_USE_PUBLISHER_RULES */
return uORB::DeviceNode::publish(meta, handle, data);
}
int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
int ret;
ret = px4_read(handle, buffer, meta->o_size);
if (ret < 0) {
return PX4_ERROR;
}
if (ret != (int)meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
return PX4_OK;
}
int uORB::Manager::orb_check(int handle, bool *updated)
{
/* Set to false here so that if `px4_ioctl` fails to false. */
*updated = false;
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
}
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
{
return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
}
int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
{
int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
@@ -508,142 +410,3 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
#endif
}
#endif /* ORB_COMMUNICATOR */
#ifdef ORB_USE_PUBLISHER_RULES
bool uORB::Manager::startsWith(const char *pre, const char *str)
{
size_t lenpre = strlen(pre),
lenstr = strlen(str);
return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
}
bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name)
{
const char **topics_ptr = rule.topics;
while (*topics_ptr) {
if (strcmp(*topics_ptr, topic_name) == 0) {
return true;
}
++topics_ptr;
}
return false;
}
void uORB::Manager::strTrim(const char **str)
{
while (**str == ' ' || **str == '\t') { ++(*str); }
}
int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
{
FILE *fp;
static const int line_len = 1024;
int ret = PX4_OK;
char *line = new char[line_len];
if (!line) {
return -ENOMEM;
}
fp = fopen(file_name, "r");
if (fp == NULL) {
delete[](line);
return -errno;
}
const char *restrict_topics_str = "restrict_topics:";
const char *module_str = "module:";
const char *ignore_others = "ignore_others:";
rule.ignore_other_topics = false;
rule.module_name = nullptr;
rule.topics = nullptr;
while (fgets(line, line_len, fp) && ret == PX4_OK) {
if (strlen(line) < 2 || line[0] == '#') {
continue;
}
if (startsWith(restrict_topics_str, line)) {
//read topics list
char *start = line + strlen(restrict_topics_str);
strTrim((const char **)&start);
char *topics = strdup(start);
int topic_len = 0, num_topics = 0;
for (int i = 0; topics[i]; ++i) {
if (topics[i] == ',' || topics[i] == '\n') {
if (topic_len > 0) {
topics[i] = 0;
++num_topics;
}
topic_len = 0;
} else {
++topic_len;
}
}
if (num_topics > 0) {
rule.topics = new const char *[num_topics + 1];
int topic = 0;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
while (topic < num_topics) {
if (*topics == 0) {
++topics;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
} else {
++topics;
}
}
rule.topics[num_topics] = nullptr;
}
} else if (startsWith(module_str, line)) {
//read module name
char *start = line + strlen(module_str);
strTrim((const char **)&start);
int len = strlen(start);
if (len > 0 && start[len - 1] == '\n') {
start[len - 1] = 0;
}
rule.module_name = strdup(start);
} else if (startsWith(ignore_others, line)) {
const char *start = line + strlen(ignore_others);
strTrim(&start);
if (startsWith("true", start)) {
rule.ignore_other_topics = true;
}
} else {
PX4_ERR("orb rules file: wrong format: %s", line);
ret = -EINVAL;
}
}
if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
PX4_ERR("Wrong format in orb publisher rules file");
ret = -EINVAL;
}
delete[](line);
fclose(fp);
return ret;
}
#endif /* ORB_USE_PUBLISHER_RULES */
platforms/common/uORB/uORBManager.hpp
+29 -48
View File
@@ -256,7 +256,7 @@ public:
* @param handle A handle returned from orb_subscribe.
* @return OK on success, PX4_ERROR otherwise with errno set accordingly.
*/
int orb_unsubscribe(int handle);
int orb_unsubscribe(int handle) { return px4_close(handle); }
/**
* Fetch data from a topic.
@@ -274,7 +274,21 @@ public:
* using the data.
* @return OK on success, PX4_ERROR otherwise with errno set accordingly.
*/
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer);
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
int ret = px4_read(handle, buffer, meta->o_size);
if (ret < 0) {
return PX4_ERROR;
}
if (ret != (int)meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
return PX4_OK;
}
/**
* Check whether a topic has been published to since the last orb_copy.
@@ -292,7 +306,12 @@ public:
* @return OK if the check was successful, PX4_ERROR otherwise with
* errno set accordingly.
*/
int orb_check(int handle, bool *updated);
int orb_check(int handle, bool *updated)
{
/* Set to false here so that if `px4_ioctl` fails to false. */
*updated = false;
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
}
/**
* Check if a topic has already been created and published (advertised)
@@ -321,8 +340,7 @@ public:
* @param interval An interval period in milliseconds.
* @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly.
*/
int orb_set_interval(int handle, unsigned interval);
int orb_set_interval(int handle, unsigned interval) { return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); }
/**
* Get the minimum interval between which updates are seen for a subscription.
@@ -381,9 +399,12 @@ private: // data members
DeviceMaster *_device_master{nullptr};
private: //class methods
Manager();
virtual ~Manager();
private: // class methods
Manager() = default;
virtual ~Manager()
{
delete _device_master;
}
#ifdef ORB_COMMUNICATOR
/**
@@ -442,46 +463,6 @@ private: //class methods
*/
virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
#endif /* ORB_COMMUNICATOR */
#ifdef ORB_USE_PUBLISHER_RULES
struct PublisherRule {
const char **topics; //null-terminated list of topic names
const char *module_name; //only this module is allowed to publish one of the topics
bool ignore_other_topics;
};
/**
* test if str starts with pre
*/
bool startsWith(const char *pre, const char *str);
/**
* find a topic in a rule
*/
bool findTopic(const PublisherRule &rule, const char *topic_name);
/**
* trim whitespace from the beginning of a string
*/
void strTrim(const char **str);
/**
* Read publisher rules from a file. It has the format:
*
* restrict_topics: <topic1>, <topic2>, <topic3>
* module: <module_name>
* [ignore_others:true]
*
* @return 0 on success, <0 otherwise
*/
int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
PublisherRule _publisher_rule;
bool _has_publisher_rules = false;
#endif /* ORB_USE_PUBLISHER_RULES */
};
#endif /* _uORBManager_hpp_ */
platforms/posix/src/px4/common/drv_hrt.cpp
+14 -12
View File
@@ -77,7 +77,7 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
static px4_sem_t _hrt_lock;
static struct work_s _hrt_work;
static hrt_abstime px4_timestart_monotonic = 0;
static hrt_abstime px4_timestart_monotonic = UINT64_MAX;
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
static LockstepScheduler *lockstep_scheduler = new LockstepScheduler();
@@ -90,7 +90,7 @@ static void hrt_call_invoke();
hrt_abstime hrt_absolute_time_offset()
{
return px4_timestart_monotonic;
return (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
}
static void hrt_lock()
@@ -136,7 +136,9 @@ hrt_abstime hrt_absolute_time()
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
// optimized case (avoid ts_to_abstime) if lockstep scheduler is used
const uint64_t abstime = lockstep_scheduler->get_absolute_time();
return abstime - px4_timestart_monotonic;
uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
return abstime - timestart_monotonic;
#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
struct timespec ts;
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
@@ -149,9 +151,7 @@ hrt_abstime hrt_absolute_time()
*/
hrt_abstime ts_to_abstime(const struct timespec *ts)
{
hrt_abstime result;
result = (hrt_abstime)(ts->tv_sec) * 1000000;
hrt_abstime result = (hrt_abstime)(ts->tv_sec) * 1000000;
result += ts->tv_nsec / 1000;
return result;
@@ -510,7 +510,8 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
if (clk_id == CLOCK_MONOTONIC) {
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
const uint64_t abstime = lockstep_scheduler->get_absolute_time();
abstime_to_ts(tp, abstime - px4_timestart_monotonic);
uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
abstime_to_ts(tp, abstime - timestart_monotonic);
return 0;
#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
#if defined(__PX4_DARWIN)
@@ -536,7 +537,7 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
} else {
const uint64_t time_us = ts_to_abstime(ts);
if (px4_timestart_monotonic == 0) {
if (px4_timestart_monotonic == UINT64_MAX) {
px4_timestart_monotonic = time_us;
}
@@ -545,10 +546,9 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
}
}
int px4_usleep(useconds_t usec)
{
if (px4_timestart_monotonic == 0) {
if (px4_timestart_monotonic == UINT64_MAX) {
// Until the time is set by the simulator, we fallback to the normal
// usleep;
return system_usleep(usec);
@@ -561,7 +561,7 @@ int px4_usleep(useconds_t usec)
unsigned int px4_sleep(unsigned int seconds)
{
if (px4_timestart_monotonic == 0) {
if (px4_timestart_monotonic == UINT64_MAX) {
// Until the time is set by the simulator, we fallback to the normal
// sleep;
return system_sleep(seconds);
@@ -577,8 +577,10 @@ int px4_pthread_cond_timedwait(pthread_cond_t *cond,
pthread_mutex_t *mutex,
const struct timespec *ts)
{
uint64_t timestart_monotonic = (px4_timestart_monotonic == UINT64_MAX) ? 0 : px4_timestart_monotonic;
const uint64_t time_us = ts_to_abstime(ts);
const uint64_t scheduled = time_us + px4_timestart_monotonic;
const uint64_t scheduled = time_us + timestart_monotonic;
return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled);
}