yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Simulator: Remove EKF2 based wait"

Browse Source 
This reverts commit 1834c156d2.
This commit is contained in:
Julian Oes
2020-01-17 13:38:29 +01:00
committed by Lorenz Meier
parent 30188d2884
commit 30edcad752
2 changed files with 73 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -596,28 +596,70 @@ 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) {
// Wait for up to 100ms for data.
int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (pret == 0) {
// Timed out, try again.
continue;
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) {
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();
#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) {
orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, nullptr);
state = State::WaitingForActuatorControls;
}
}
#endif
}
}
@@ -710,7 +752,7 @@ void Simulator::run()
} else {
::close(_fd);
system_usleep(100);
system_sleep(1);
}
}
@@ -761,6 +803,10 @@ 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);
@@ -823,6 +869,9 @@ void Simulator::run()
}
orb_unsubscribe(_actuator_outputs_sub);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
orb_unsubscribe(_ekf2_timestamps_sub);
#endif
}
#ifdef ENABLE_UART_RC_INPUT