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

sitl: use lockstep components API

Browse Source 
- avoids the need for ekf2_timestamp publications by q and lpe
- adds logger to the lockstep cycle and makes it poll on ekf2_timestamps
  or vehicle_attitude. This avoids dropped samples (required for replay).
This commit is contained in:
Beat Küng
2020-06-18 11:43:22 +02:00
committed by Daniel Agar
parent 5378d1468f
commit 71dcf8d619
10 changed files with 102 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
-8
View File
@@ -273,14 +273,6 @@ private:
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4::atomic<bool> _has_initialized {false};
int _ekf2_timestamps_sub{-1};
enum class State {
WaitingForFirstEkf2Timestamp = 0,
WaitingForActuatorControls = 1,
WaitingForEkf2Timestamp = 2,
};
#endif
DEFINE_PARAMETERS(
src/modules/simulator/simulator_mavlink.cpp
+16 -65
View File
@@ -604,71 +604,29 @@ void Simulator::send()
fds_actuator_outputs[0].fd = _actuator_outputs_sub;
fds_actuator_outputs[0].events = POLLIN;
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4_pollfd_struct_t fds_ekf2_timestamps[1] = {};
fds_ekf2_timestamps[0].fd = _ekf2_timestamps_sub;
fds_ekf2_timestamps[0].events = POLLIN;
State state = State::WaitingForFirstEkf2Timestamp;
#endif
while (true) {
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
// Wait for up to 100ms for data.
int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
if (state == State::WaitingForActuatorControls) {
#endif
// Wait for up to 100ms for data.
int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
if (pret == 0) {
// Timed out, try again.
continue;
}
if (pret < 0) {
PX4_ERR("poll error %s", strerror(errno));
continue;
}
if (fds_actuator_outputs[0].revents & POLLIN) {
// Got new data to read, update all topics.
parameters_update(false);
_vehicle_status_sub.update(&_vehicle_status);
send_controls();
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
state = State::WaitingForEkf2Timestamp;
#endif
}
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (pret == 0) {
// Timed out, try again.
continue;
}
#endif
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (state == State::WaitingForFirstEkf2Timestamp || state == State::WaitingForEkf2Timestamp) {
int pret = px4_poll(&fds_ekf2_timestamps[0], 1, 100);
if (pret == 0) {
// Timed out, try again.
continue;
}
if (pret < 0) {
PX4_ERR("poll error %s", strerror(errno));
continue;
}
if (fds_ekf2_timestamps[0].revents & POLLIN) {
ekf2_timestamps_s timestamps;
orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, &timestamps);
state = State::WaitingForActuatorControls;
}
if (pret < 0) {
PX4_ERR("poll error %s", strerror(errno));
continue;
}
#endif
if (fds_actuator_outputs[0].revents & POLLIN) {
// Got new data to read, update all topics.
parameters_update(false);
_vehicle_status_sub.update(&_vehicle_status);
send_controls();
// Wait for other modules, such as logger or ekf2
px4_lockstep_wait_for_components();
}
}
}
@@ -812,10 +770,6 @@ void Simulator::run()
// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
_ekf2_timestamps_sub = orb_subscribe(ORB_ID(ekf2_timestamps));
#endif
// got data from simulator, now activate the sending thread
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr);
pthread_attr_destroy(&sender_thread_attr);
@@ -878,9 +832,6 @@ void Simulator::run()
}
orb_unsubscribe(_actuator_outputs_sub);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
orb_unsubscribe(_ekf2_timestamps_sub);
#endif
}
#ifdef ENABLE_UART_RC_INPUT