From 6d7fb232dd1d9574884e2caf03cf00257e907ec9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 13 Nov 2018 15:18:23 +0100 Subject: [PATCH] simulator: added TCP, general cleanup, lockstep This adds the option to connect over UDP or TCP for mavlink to the SITL simulator. Also, this includes a bunch of general cleanup and refactoring of the simulator interface code. For instance sending of mavlink messages was put into separate functions and unneccessary comments were removed. Also, this now sets the timestamp sent by the SITL simulator in the HIL_SENSOR message in order to enable lockstep. --- src/modules/simulator/simulator.cpp | 88 ++++--- src/modules/simulator/simulator.h | 21 +- src/modules/simulator/simulator_mavlink.cpp | 243 ++++++++++---------- 3 files changed, 181 insertions(+), 171 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 077a6ca837..339c7f09f6 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2018 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 @@ -33,7 +34,9 @@ /** * @file simulator.cpp - * A device simulator + * + * This module interfaces via MAVLink to a software in the loop simulator (SITL) + * such as jMAVSim or Gazebo. */ #include @@ -141,47 +144,54 @@ void Simulator::parameters_update(bool force) int Simulator::start(int argc, char *argv[]) { - int ret = 0; - int udp_port = 14560; _instance = new Simulator(); if (_instance) { drv_led_start(); + InternetProtocol ip = InternetProtocol::UDP; + unsigned port = 14560; + if (argc == 5 && strcmp(argv[3], "-u") == 0) { - udp_port = atoi(argv[4]); + ip = InternetProtocol::UDP; + port = atoi(argv[4]); + } + + if (argc == 5 && strcmp(argv[3], "-t") == 0) { + ip = InternetProtocol::TCP; + port = atoi(argv[4]); } if (argv[2][1] == 's') { _instance->initializeSensorData(); #ifndef __PX4_QURT // Update sensor data - _instance->pollForMAVLinkMessages(false, udp_port); + _instance->pollForMAVLinkMessages(false, ip, port); #endif } else if (argv[2][1] == 'p') { // Update sensor data - _instance->pollForMAVLinkMessages(true, udp_port); + _instance->pollForMAVLinkMessages(true, ip, port); } else { _instance->initializeSensorData(); - _instance->_initialized = true; } + return 0; + } else { PX4_WARN("Simulator creation failed"); - ret = 1; + return 1; } - - return ret; } static void usage() { - PX4_WARN("Usage: simulator {start -[spt] [-u udp_port] |stop}"); + PX4_WARN("Usage: simulator {start -[sp] [-u udp_port / -t tcp_port] |stop}"); PX4_WARN("Simulate raw sensors: simulator start -s"); PX4_WARN("Publish sensors combined: simulator start -p"); - PX4_WARN("Dummy unit test data: simulator start -t"); + PX4_WARN("Connect using UDP: simulator start -u udp_port"); + PX4_WARN("Connect using TCP: simulator start -t tcp_port"); } __BEGIN_DECLS @@ -192,41 +202,27 @@ extern "C" { int simulator_main(int argc, char *argv[]) { - int ret = 0; + if (argc == 5 && strcmp(argv[1], "start") == 0) { - if (argc > 2 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || - strcmp(argv[2], "-p") == 0 || - strcmp(argv[2], "-t") == 0) { + if (g_sim_task >= 0) { + PX4_WARN("Simulator already started"); + return 0; + } - if (g_sim_task >= 0) { - warnx("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() && Simulator::getInstance()->isInitialized()) { + break; + + } else { + system_sleep(1); } - - // enable lockstep support - px4_enable_sim_lockstep(); - - g_sim_task = px4_task_spawn_cmd("simulator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 1500, - Simulator::start, - argv); - - // now wait for the command to complete - while (!px4_exit_requested()) { - if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) { - break; - - } else { - px4_usleep(100000); - } - } - - } else { - usage(); - ret = -EINVAL; } } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { @@ -240,10 +236,10 @@ extern "C" { } else { usage(); - ret = -EINVAL; + return 1; } - return ret; + return 0; } } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index f070285df1..8289e7c4ec 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 Pro Dev Team. All rights reserved. + * Copyright (c) 2018 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 @@ -32,9 +32,12 @@ * ****************************************************************************/ + /** * @file simulator.h - * A device simulator + * + * This module interfaces via MAVLink to a software in the loop simulator (SITL) + * such as jMAVSim or Gazebo. */ #pragma once @@ -65,10 +68,10 @@ #include #include #include + namespace simulator { -// FIXME - what is the endianness of these on actual device? #pragma pack(push, 1) struct RawAccelData { float temperature; @@ -187,7 +190,8 @@ protected: RType _buf[2]; }; -}; +} // namespace simulator + class Simulator : public ModuleParams { @@ -381,10 +385,17 @@ private: ) + enum class InternetProtocol { + TCP, + UDP + }; + void poll_topics(); void handle_message(mavlink_message_t *msg, bool publish); void send_controls(); - void pollForMAVLinkMessages(bool publish, int udp_port); + void send_heartbeat(); + void request_hil_state_quaternion(); + void pollForMAVLinkMessages(bool publish, InternetProtocol ip, int port); void pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index); void send_mavlink_message(const mavlink_message_t &aMsg); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a6a131f585..56a559f399 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 Pro Dev Team. All rights reserved. + * Copyright (c) 2018 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 @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + #include #include #include @@ -43,15 +44,13 @@ #include #include #include +#include #include #include #include #include -#define SEND_INTERVAL 20 -#define UDP_PORT 14560 - #ifdef ENABLE_UART_RC_INPUT #ifndef B460800 #define B460800 460800 @@ -65,12 +64,12 @@ static int openUart(const char *uart_name, int baud); #endif static int _fd; -static unsigned char _buf[1024]; -sockaddr_in _srcaddr; -static socklen_t _addrlen = sizeof(_srcaddr); +static unsigned char _buf[2048]; +static sockaddr_in _srcaddr; +static unsigned _addrlen = sizeof(_srcaddr); static hrt_abstime batt_sim_start = 0; -const unsigned mode_flag_armed = 128; // following MAVLink spec +const unsigned mode_flag_armed = 128; const unsigned mode_flag_custom = 1; using namespace simulator; @@ -175,11 +174,13 @@ void Simulator::send_controls() } mavlink_hil_actuator_controls_t hil_act_control = {}; + hil_act_control.time_usec = _actuators[i].timestamp + hrt_absolute_time_offset(); + mavlink_message_t message = {}; pack_actuator_message(hil_act_control, i); mavlink_msg_hil_actuator_controls_encode(0, 200, &message, &hil_act_control); - send_mavlink_message(message); + send_mavlink_message(message); } } @@ -260,7 +261,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { RawGPSData gps = {}; - gps.timestamp = gps_sim->time_usec; + gps.timestamp = hrt_absolute_time(); gps.lat = gps_sim->lat; gps.lon = gps_sim->lon; gps.alt = gps_sim->alt; @@ -300,7 +301,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) float step = diff / 4000.0f; if (step > 1.1f || step < 0.9f) { - PX4_INFO("diff: %llu, step: %.2f", diff, step); + PX4_INFO("time_usec: %llu, diff: %llu, step: %.2f", now_us, diff, step); } last_time = now_us; @@ -506,11 +507,9 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint16_t bufLen = 0; - // convery mavlink message to raw data bufLen = mavlink_msg_to_send_buffer(buf, &aMsg); - // send data - ssize_t len = sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, _addrlen); + ssize_t len = sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr)); if (len <= 0) { PX4_WARN("Failed sending mavlink message"); @@ -546,37 +545,42 @@ void *Simulator::sending_trampoline(void * /*unused*/) void Simulator::send() { - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _actuator_outputs_sub[0]; - fds[0].events = POLLIN; - - - // set the threads name #ifdef __PX4_DARWIN pthread_setname_np("sim_send"); #else pthread_setname_np(pthread_self(), "sim_send"); #endif - int pret; + // Before starting, we ought to send a heartbeat to initiate the SITL + // simulator to start sending sensor data which will set the time and + // get everything rolling. + // Without this, we get stuck at px4_poll which waits for a time update. + send_heartbeat(); + + // We then send the controls once which will notify the SITL simulator + // that our time is 0. + send_controls(); + + px4_pollfd_struct_t fds[1] = {}; + fds[0].fd = _actuator_outputs_sub[0]; + fds[0].events = POLLIN; while (true) { - // wait for up to 100ms for data - pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + // Wait for up to 100ms for data. + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - // timed out if (pret == 0) { + // Timed out, try again. continue; } - // this is undesirable but not much we can do if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); + PX4_ERR("poll error %s", strerror(errno)); continue; } if (fds[0].revents & POLLIN) { - // got new data to read, update all topics + // Got new data to read, update all topics. parameters_update(false); poll_topics(); send_controls(); @@ -584,9 +588,30 @@ void Simulator::send() } } +void Simulator::request_hil_state_quaternion() +{ + mavlink_command_long_t cmd_long = {}; + mavlink_message_t message = {}; + cmd_long.command = MAV_CMD_SET_MESSAGE_INTERVAL; + cmd_long.param1 = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; + cmd_long.param2 = 5e3; + mavlink_msg_command_long_encode(0, 50, &message, &cmd_long); + send_mavlink_message(message); +} + +void Simulator::send_heartbeat() +{ + mavlink_heartbeat_t hb = {}; + mavlink_message_t message = {}; + hb.autopilot = 12; + hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; + mavlink_msg_heartbeat_encode(0, 50, &message, &hb); + send_mavlink_message(message); +} + void Simulator::initializeSensorData() { - // write sensor data to memory so that drivers can copy data from there + // Write sensor data to memory so that drivers can copy data from there. RawMPUData mpu = {}; mpu.accel_z = CONSTANTS_ONE_G; @@ -616,38 +641,79 @@ void Simulator::initializeSensorData() write_airspeed_data(&airspeed); } -void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) +void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int port) { - // set the threads name #ifdef __PX4_DARWIN pthread_setname_np("sim_rcv"); #else pthread_setname_np(pthread_self(), "sim_rcv"); #endif - // udp socket data - struct sockaddr_in _myaddr; - - // try to setup udp socket for communcation with simulator - memset((char *)&_myaddr, 0, sizeof(_myaddr)); + struct sockaddr_in _myaddr {}; _myaddr.sin_family = AF_INET; _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - _myaddr.sin_port = htons(udp_port); + _myaddr.sin_port = htons(port); - if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - PX4_ERR("create socket failed (%i)", errno); - return; + if (ip == InternetProtocol::UDP) { + + if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_ERR("Creating UDP socket failed: %s", strerror(errno)); + return; + } + + if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_ERR("bind for UDP port %i failed (%i)", port, errno); + return; + } + + PX4_INFO("Waiting for simulator to connect on UDP port %u", port); + + while (true) { + // Once we receive something, we're most probably good and can carry on. + int len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + + if (len > 0) { + break; + + } else { + system_sleep(1); + } + } + + PX4_INFO("Simulator connected on UDP port %u.", port); + + } else { + if ((_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + PX4_ERR("Creating TCP socket failed: %s", strerror(errno)); + return; + } + + PX4_INFO("Waiting for simulator to connect on TCP port %u", port); + + while (true) { + int ret = connect(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)); + + if (ret == 0) { + break; + + } else { + system_sleep(1); + } + } + + PX4_INFO("Simulator connected on TCP port %u.", port); + + int yes = 1; + int result = setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, (char *) &yes, sizeof(int)); + + if (result != 0) { + PX4_ERR("setsockopt failed: %s", strerror(errno)); + } } - if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { - PX4_ERR("bind for UDP port %i failed (%i)", udp_port, errno); - return; - } - - // create a thread for sending data to the simulator + // 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, PX4_STACK_ADJUSTED(4000)); @@ -655,7 +721,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) struct sched_param param; (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_DEFAULT + 40; + param.sched_priority = SCHED_PRIORITY_DEFAULT; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); struct pollfd fds[2]; @@ -683,53 +749,6 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) #endif - int len = 0; - - // wait for first data from simulator and respond with first controls - // this is important for the UDP communication to work - int pret = -1; - PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..", udp_port); - fflush(stdout); - - bool no_sim_data = true; - - while (!px4_exit_requested() && no_sim_data) { - pret = ::poll(&fds[0], fd_count, 100); - - if (fds[0].revents & POLLIN) { - - len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); - // send hearbeat - mavlink_heartbeat_t hb = {}; - mavlink_message_t message = {}; - hb.autopilot = 12; - hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; - mavlink_msg_heartbeat_encode(0, 50, &message, &hb); - send_mavlink_message(message); - - if (len > 0) { - mavlink_message_t msg; - mavlink_status_t udp_status = {}; - - for (int i = 0; i < len; i++) { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) { - // have a message, handle it - handle_message(&msg, publish); - - if (msg.msgid != 0) { - PX4_INFO("Got initial simulation data, running sim.."); - no_sim_data = false; - } - } - } - } - } - } - - if (px4_exit_requested()) { - return; - } - // subscribe to topics for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { _actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i); @@ -741,51 +760,37 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr); pthread_attr_destroy(&sender_thread_attr); - mavlink_status_t udp_status = {}; - - const unsigned max_wait_ms = 4; - - //send MAV_CMD_SET_MESSAGE_INTERVAL for HIL_STATE_QUATERNION ground truth - mavlink_command_long_t cmd_long = {}; - mavlink_message_t message = {}; - cmd_long.command = MAV_CMD_SET_MESSAGE_INTERVAL; - cmd_long.param1 = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; - cmd_long.param2 = 5e3; - mavlink_msg_command_long_encode(0, 50, &message, &cmd_long); - send_mavlink_message(message); + mavlink_status_t mavlink_status = {}; + // Request HIL_STATE_QUATERNION for ground truth. + request_hil_state_quaternion(); _initialized = true; - // wait for new mavlink messages to arrive while (true) { - pret = ::poll(&fds[0], fd_count, max_wait_ms); + // wait for new mavlink messages to arrive + int pret = ::poll(&fds[0], fd_count, 4); - //timed out if (pret == 0) { - + // Timed out. continue; } - // this is undesirable but not much we can do if (pret < 0) { PX4_WARN("simulator mavlink: poll error %d, %d", pret, errno); - // sleep a bit before next try - px4_usleep(100000); continue; } - // got data from simulator if (fds[0].revents & POLLIN) { - len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + + int len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); if (len > 0) { mavlink_message_t msg; for (int i = 0; i < len; i++) { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) { - // have a message, handle it + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &mavlink_status)) { handle_message(&msg, publish); } } @@ -867,8 +872,7 @@ int openUart(const char *uart_name, int baud) case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", - baud); + PX4_ERR("Unsupported baudrate: %d", baud); return -EINVAL; } @@ -918,7 +922,6 @@ int openUart(const char *uart_name, int baud) int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) { - uint64_t timestamp = hrt_absolute_time(); if ((imu->fields_updated & 0x1FFF) != 0x1FFF) {