diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 60b7683ac4..4064b6e410 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -146,6 +146,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f)); if (fabsf(status.states[index]) > test_limit + test_uncertainty) { + PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)test_limit, + (double)test_uncertainty); + if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index b072ff5072..e69a3041f4 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -174,7 +175,10 @@ public: void set_ip(InternetProtocol ip); void set_port(unsigned port); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) bool has_initialized() {return _has_initialized.load(); } +#endif private: Simulator() : @@ -287,7 +291,8 @@ private: static void *sending_trampoline(void *); - mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators); + mavlink_hil_actuator_controls_t actuator_controls_from_outputs(); + // uORB publisher handlers uORB::Publication _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; @@ -298,6 +303,8 @@ private: // uORB subscription handlers int _actuator_outputs_sub{-1}; + actuator_outputs_s _actuator_outputs{}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // hil map_ref data @@ -315,6 +322,14 @@ private: #if defined(ENABLE_LOCKSTEP_SCHEDULER) px4::atomic _has_initialized {false}; + + int _ekf2_timestamps_sub{-1}; + + enum class State { + WaitingForFirstEkf2Timestamp = 0, + WaitingForActuatorControls = 1, + WaitingForEkf2Timestamp = 2, + }; #endif DEFINE_PARAMETERS( diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d8a048088a..c948dd3723 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -74,7 +74,7 @@ const unsigned mode_flag_custom = 1; using namespace simulator; using namespace time_literals; -mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const actuator_outputs_s &actuators) +mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs() { mavlink_hil_actuator_controls_t msg{}; @@ -125,14 +125,14 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const } for (unsigned i = 0; i < 16; i++) { - if (actuators.output[i] > PWM_DEFAULT_MIN / 2) { + if (_actuator_outputs.output[i] > PWM_DEFAULT_MIN / 2) { if (i < n) { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ - msg.controls[i] = (actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } else { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ - msg.controls[i] = (actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } } else { @@ -145,14 +145,14 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 16; i++) { - if (actuators.output[i] > PWM_DEFAULT_MIN / 2) { + if (_actuator_outputs.output[i] > PWM_DEFAULT_MIN / 2) { if (i != 4) { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for normal channels */ - msg.controls[i] = (actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); } else { /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for throttle */ - msg.controls[i] = (actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); } } else { @@ -171,24 +171,17 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const void Simulator::send_controls() { - // copy new actuator data if available - bool updated = false; - orb_check(_actuator_outputs_sub, &updated); + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs); - if (updated) { - actuator_outputs_s actuators{}; - orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &actuators); + if (_actuator_outputs.timestamp > 0) { + mavlink_hil_actuator_controls_t hil_act_control = actuator_controls_from_outputs(); - if (actuators.timestamp > 0) { - const mavlink_hil_actuator_controls_t hil_act_control = actuator_controls_from_outputs(actuators); + mavlink_message_t message{}; + mavlink_msg_hil_actuator_controls_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hil_act_control); - mavlink_message_t message{}; - mavlink_msg_hil_actuator_controls_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hil_act_control); + PX4_DEBUG("sending controls t=%ld (%ld)", _actuator_outputs.timestamp, hil_act_control.time_usec); - PX4_DEBUG("sending controls t=%ld (%ld)", actuators.timestamp, hil_act_control.time_usec); - - send_mavlink_message(message); - } + send_mavlink_message(message); } } @@ -590,30 +583,74 @@ void Simulator::send() // Without this, we get stuck at px4_poll which waits for a time update. send_heartbeat(); - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _actuator_outputs_sub; - fds[0].events = POLLIN; + px4_pollfd_struct_t fds_actuator_outputs[1] = {}; + 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[0], (sizeof(fds) / sizeof(fds[0])), 100); - if (pret == 0) { - // Timed out, try again. - continue; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + + 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; +#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; + } } - if (fds[0].revents & POLLIN) { - // Got new data to read, update all topics. - parameters_update(false); - _vehicle_status_sub.update(&_vehicle_status); - send_controls(); - } +#endif } } @@ -757,6 +794,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); @@ -819,6 +860,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 diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 55ea39153d..3a8d2eec7d 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -7,6 +7,7 @@ import errno import os import psutil import subprocess +import sys test_matrix = [ @@ -42,7 +43,7 @@ class Runner: datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ") ), 'w') else: - f = subprocess.STDOUT + f = None print("Running: {}".format(" ".join([self.cmd] + self.args))) @@ -50,8 +51,8 @@ class Runner: [self.cmd] + self.args, cwd=self.cwd, env=self.env, - # FIXME: this is currently not working - # stdout=subprocess.STDOUT + stdout=f, + stderr=f ) atexit.register(self.stop) @@ -199,12 +200,12 @@ def main(): help="Directory for log files, stdout if not provided") parser.add_argument("--speed-factor", default=1, help="How fast to run the simulation") - parser.add_argument("--gui", default=False, + parser.add_argument("--gui", default=False, action='store_true', help="Display gzclient with") args = parser.parse_args() if not is_everything_ready(): - return 1 + sys.exit(1) overall_success = True @@ -253,7 +254,7 @@ def main(): print("Overall result: {}". format("SUCCESS" if overall_success else "FAIL")) - return 0 if overall_success else 1 + sys.exit(0 if overall_success else 1) if __name__ == '__main__': diff --git a/test/mavsdk_tests/test_mission_multicopter.cpp b/test/mavsdk_tests/test_mission_multicopter.cpp index c2d4e24e77..8f37db0e82 100644 --- a/test/mavsdk_tests/test_mission_multicopter.cpp +++ b/test/mavsdk_tests/test_mission_multicopter.cpp @@ -23,7 +23,7 @@ TEST_CASE("Takeoff and land", "[multicopter][vtol]") tester.wait_until_disarmed(); } -TEST_CASE("Fly a square missions", "[multicopter][vtol]") +TEST_CASE("Fly square missions", "[multicopter][vtol]") { AutopilotTester tester; tester.connect(connection_url);