diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 702966cf6e..0c02da247f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 43c97df668..b2ebc880cd 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -276,7 +276,7 @@ private: void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); void update_gps(mavlink_hil_gps_t *gps_sim); - static int sending_trampoline(int argc, char *argv[]); + static void *sending_trampoline(void *); void send(); #endif }; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 88893f9c4a..e4403fd221 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -33,7 +33,6 @@ #include #include #include -#include #include "simulator.h" #include "errno.h" #include @@ -251,9 +250,9 @@ void Simulator::poll_actuators() { } } -int Simulator::sending_trampoline(int argv, char *argc[]) { +void *Simulator::sending_trampoline(void *) { _instance->send(); - return 0; + return 0; // why do I have to put this??? } void Simulator::send() { @@ -327,6 +326,21 @@ void Simulator::updateSamples() return; } + // create a thread for sending data to the simulator + pthread_t sender_thread; + + // initialize threads + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, 1000); + + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + // setup serial connection to autopilot (used to get manual controls) int serial_fd = openUart(PIXHAWK_DEVICE, 115200); @@ -367,15 +381,9 @@ void Simulator::updateSamples() // subscribe to topics _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); - char * argv[2] = { nullptr, nullptr }; - // got data from simulator, now activate the sending thread - px4_task_spawn_cmd("simulator-send", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1000, - (px4_main_t)&Simulator::sending_trampoline, - (char *const *)argv); + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); // wait for new mavlink messages to arrive while (true) { diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 4fe4d499bd..044622c4af 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -114,7 +114,7 @@ private: uint8_t *_data; /**< allocated object buffer */ hrt_abstime _last_update; /**< time the object was last updated */ volatile unsigned _generation; /**< object generation count */ - pid_t _publisher; /**< if nonzero, current publisher */ + unsigned long _publisher; /**< if nonzero, current publisher */ const int _priority; /**< priority of topic */ SubscriberData *filp_to_sd(device::file_t *filp); diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index ac4c6b62bf..c65fadcc1a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -278,21 +278,9 @@ void px4_show_tasks() __BEGIN_DECLS -int px4_getpid() +unsigned long px4_getpid() { - pthread_t pid = pthread_self(); - - if (pid == _shell_task_id) - return SHELL_TASK_ID; - - // Get pthread ID from the opaque ID - for (int i=0; i