fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 23:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulator udp port from command line

Browse Source
This commit is contained in:
korotkoves
2016-05-13 13:23:20 +02:00
committed by Lorenz Meier
parent adfe946f72
commit 22e3ce7d7a
3 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.cpp
+9 -4
View File
@@ -44,6 +44,7 @@
#include <systemlib/err.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <drivers/drv_led.h>
@@ -124,21 +125,25 @@ void Simulator::write_airspeed_data(void *buf)
int Simulator::start(int argc, char *argv[])
{
int ret = 0;
int udp_port = 0;
_instance = new Simulator();
if (_instance) {
drv_led_start();
if (argc == 5 && strcmp(argv[3], "-u") == 0)
udp_port = atoi(argv[4]);
if (argv[2][1] == 's') {
_instance->initializeSensorData();
#ifndef __PX4_QURT
// Update sensor data
_instance->pollForMAVLinkMessages(false);
_instance->pollForMAVLinkMessages(false,udp_port);
#endif
} else if (argv[2][1] == 'p') {
// Update sensor data
_instance->pollForMAVLinkMessages(true);
_instance->pollForMAVLinkMessages(true,udp_port);
} else {
_instance->initializeSensorData();
@@ -155,7 +160,7 @@ int Simulator::start(int argc, char *argv[])
static void usage()
{
PX4_WARN("Usage: simulator {start -[spt] |stop}");
PX4_WARN("Usage: simulator {start -[spt] [-u udp_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");
@@ -171,7 +176,7 @@ extern "C" {
{
int ret = 0;
if (argc == 3 && 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) {
src/modules/simulator/simulator.h
+1 -1
View File
@@ -314,7 +314,7 @@ private:
void poll_topics();
void handle_message(mavlink_message_t *msg, bool publish);
void send_controls();
void pollForMAVLinkMessages(bool publish);
void pollForMAVLinkMessages(bool publish, int udp_port);
void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
src/modules/simulator/simulator_mavlink.cpp
+6 -4
View File
@@ -462,7 +462,7 @@ void Simulator::initializeSensorData()
write_airspeed_data(&airspeed);
}
void Simulator::pollForMAVLinkMessages(bool publish)
void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
{
// set the threads name
#ifdef __PX4_DARWIN
@@ -473,13 +473,15 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// udp socket data
struct sockaddr_in _myaddr;
const int _port = UDP_PORT;
if (udp_port < 1)
udp_port = UDP_PORT;
// try to setup udp socket for communcation with simulator
memset((char *)&_myaddr, 0, sizeof(_myaddr));
_myaddr.sin_family = AF_INET;
_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
_myaddr.sin_port = htons(_port);
_myaddr.sin_port = htons(udp_port);
if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
PX4_WARN("create socket failed\n");
@@ -532,7 +534,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
// 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. Please start the flight simulator to proceed..");
PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..",udp_port);
uint64_t pstart_time = 0;