From 8817f172eaab6cfea7e0a832b005387f2f59e24b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Nov 2019 14:15:13 +0100 Subject: [PATCH] simulator: cleanup and wait for startup Unfortunately this commit contains two things: 1. Some cleanup and renaiming. 2. An additional wait until lockstep has been initialized. By waiting until HIL_SENSOR messages arrive including timestamps we stop the startup script and prevent other modules from running until time is set up. This should resolve some busy waiting by various modules and prevent races on initialization (e.g. the landing state being subscribed by mavlink before being published by the land detector). --- src/modules/simulator/simulator.cpp | 100 +++++++++----------- src/modules/simulator/simulator.h | 14 ++- src/modules/simulator/simulator_mavlink.cpp | 12 ++- 3 files changed, 67 insertions(+), 59 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index d655fe5048..fc910959da 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2015 Mark Charlebois. All rights reserved. - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,14 +42,7 @@ #include #include #include -#include -#include #include -#include -#include -#include -#include -#include #include #include "simulator.h" @@ -106,10 +99,8 @@ int Simulator::start(int argc, char *argv[]) } #ifndef __PX4_QURT - // Update sensor data - _instance->poll_for_MAVLink_messages(); + _instance->run(); #endif - return 0; } else { @@ -130,58 +121,61 @@ void Simulator::set_port(unsigned port) static void usage() { - PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}"); - PX4_WARN("Start simulator: simulator start"); - PX4_WARN("Connect using UDP: simulator start -u udp_port"); - PX4_WARN("Connect using TCP: simulator start -c tcp_port"); + PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}"); + PX4_INFO("Start simulator: simulator start"); + PX4_INFO("Connect using UDP: simulator start -u udp_port"); + PX4_INFO("Connect using TCP: simulator start -c tcp_port"); } __BEGIN_DECLS extern int simulator_main(int argc, char *argv[]); __END_DECLS -extern "C" { - int simulator_main(int argc, char *argv[]) - { - if (argc > 1 && strcmp(argv[1], "start") == 0) { +int simulator_main(int argc, char *argv[]) +{ + if (argc > 1 && strcmp(argv[1], "start") == 0) { - if (g_sim_task >= 0) { - PX4_WARN("Simulator already started"); - return 0; - } - - g_sim_task = px4_task_spawn_cmd("simulator", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1500, - Simulator::start, - argv); - - while (true) { - if (Simulator::getInstance()) { - break; - - } else { - system_usleep(100); - } - } - - } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_sim_task < 0) { - PX4_WARN("Simulator not running"); - - } else { - px4_task_delete(g_sim_task); - g_sim_task = -1; - } - - } else { - usage(); - return 1; + if (g_sim_task >= 0) { + PX4_WARN("Simulator already started"); + return 0; } - return 0; + g_sim_task = px4_task_spawn_cmd("simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1500, + Simulator::start, + argv); + +#if !defined(__PX4_QURT) && defined(ENABLE_LOCKSTEP_SCHEDULER) + + // We want to prevent the rest of the startup script from running until time + // is initialized by the HIL_SENSOR messages from the simulator. + while (true) { + if (Simulator::getInstance() && Simulator::getInstance()->has_initialized()) { + break; + } + + system_usleep(100); + } + +#endif + + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_sim_task < 0) { + PX4_WARN("Simulator not running"); + return 1; + + } else { + px4_task_delete(g_sim_task); + g_sim_task = -1; + } + + } else { + usage(); + return 1; } + return 0; } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 2f73ecb92e..b072ff5072 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2015 Mark Charlebois. All rights reserved. - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -173,6 +174,7 @@ public: void set_ip(InternetProtocol ip); void set_port(unsigned port); + bool has_initialized() {return _has_initialized.load(); } private: Simulator() : @@ -261,8 +263,7 @@ private: #ifndef __PX4_QURT - mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators); - + void run(); void handle_message(const mavlink_message_t *msg); void handle_message_distance_sensor(const mavlink_message_t *msg); void handle_message_hil_gps(const mavlink_message_t *msg); @@ -275,7 +276,6 @@ private: void handle_message_vision_position_estimate(const mavlink_message_t *msg); void parameters_update(bool force); - void poll_for_MAVLink_messages(); void request_hil_state_quaternion(); void send(); @@ -287,6 +287,8 @@ private: static void *sending_trampoline(void *); + mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators); + // uORB publisher handlers uORB::Publication _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; @@ -311,6 +313,10 @@ private: // uORB data containers vehicle_status_s _vehicle_status{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + px4::atomic _has_initialized {false}; +#endif + DEFINE_PARAMETERS( (ParamFloat) _param_sim_bat_drain, ///< battery drain interval (ParamFloat) _battery_min_percentage, //< minimum battery percentage diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index bce060cb9b..d8a048088a 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -2,7 +2,7 @@ * * Copyright (c) 2015 Mark Charlebois. All rights reserved. * Copyright (c) 2016 Anton Matosov. All rights reserved. - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -374,6 +374,14 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) _last_battery_timestamp = now_us; } + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + + if (!_has_initialized.load()) { + _has_initialized.store(true); + } + +#endif } void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg) @@ -630,7 +638,7 @@ void Simulator::send_heartbeat() send_mavlink_message(message); } -void Simulator::poll_for_MAVLink_messages() +void Simulator::run() { #ifdef __PX4_DARWIN pthread_setname_np("sim_rcv");