Mark Charlebois
8 years ago
committed by
Lorenz Meier
4 changed files with 297 additions and 190 deletions
@ -0,0 +1,82 @@
@@ -0,0 +1,82 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright notice, this |
||||
* list of conditions and the following disclaimer. |
||||
* |
||||
* 2. Redistributions in binary form must reproduce the above copyright notice, |
||||
* this list of conditions and the following disclaimer in the documentation |
||||
* and/or other materials provided with the distribution. |
||||
* |
||||
* 3. Neither the name of the copyright holder nor the names of its contributors |
||||
* may be used to endorse or promote products derived from this software without |
||||
* specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "microRTPS_transport.h" |
||||
|
||||
#include <cinttypes> |
||||
#include <cstdio> |
||||
#include <ctime> |
||||
#include <pthread.h> |
||||
#include <termios.h> |
||||
|
||||
#include <microcdr/microCdr.h> |
||||
#include <px4_config.h> |
||||
#include <px4_getopt.h> |
||||
#include <px4_posix.h> |
||||
#include <px4_tasks.h> |
||||
#include <px4_time.h> |
||||
#include <uORB/uORB.h> |
||||
|
||||
#define BUFFER_SIZE 1024 |
||||
#define UPDATE_TIME_MS 0 |
||||
#define LOOPS 10000 |
||||
#define SLEEP_MS 1 |
||||
#define BAUDRATE 460800 |
||||
#define DEVICE "/dev/ttyACM0" |
||||
#define POLL_MS 1 |
||||
#define DEFAULT_RECV_PORT 2019 |
||||
#define DEFAULT_SEND_PORT 2020 |
||||
|
||||
void* send(void *data); |
||||
void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop); |
||||
|
||||
struct options { |
||||
enum class eTransports |
||||
{ |
||||
UART, |
||||
UDP |
||||
}; |
||||
eTransports transport = options::eTransports::UART; |
||||
char device[64] = DEVICE; |
||||
int update_time_ms = UPDATE_TIME_MS; |
||||
int loops = LOOPS; |
||||
int sleep_ms = SLEEP_MS; |
||||
uint32_t baudrate = BAUDRATE; |
||||
int poll_ms = POLL_MS; |
||||
uint16_t recv_port = DEFAULT_RECV_PORT; |
||||
uint16_t send_port = DEFAULT_SEND_PORT; |
||||
}; |
||||
|
||||
extern struct options _options; |
||||
extern bool _should_exit_task; |
||||
extern Transport_node *transport_node; |
||||
|
||||
@ -0,0 +1,210 @@
@@ -0,0 +1,210 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright notice, this |
||||
* list of conditions and the following disclaimer. |
||||
* |
||||
* 2. Redistributions in binary form must reproduce the above copyright notice, |
||||
* this list of conditions and the following disclaimer in the documentation |
||||
* and/or other materials provided with the distribution. |
||||
* |
||||
* 3. Neither the name of the copyright holder nor the names of its contributors |
||||
* may be used to endorse or promote products derived from this software without |
||||
* specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "microRTPS_transport.h" |
||||
#include "microRTPS_client.h" |
||||
|
||||
#include <cinttypes> |
||||
#include <cstdio> |
||||
#include <ctime> |
||||
#include <termios.h> |
||||
|
||||
#include <px4_config.h> |
||||
#include <px4_getopt.h> |
||||
#include <px4_posix.h> |
||||
#include <px4_tasks.h> |
||||
#include <px4_time.h> |
||||
|
||||
extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); |
||||
|
||||
static int _rtps_task = -1; |
||||
bool _should_exit_task = false; |
||||
Transport_node *transport_node = nullptr; |
||||
struct options _options; |
||||
|
||||
static void usage(const char *name) |
||||
{ |
||||
PX4_INFO("usage: %s start|stop [options]\n\n" |
||||
" -t <transport> [UART|UDP] Default UART\n" |
||||
" -d <device> UART device. Default /dev/ttyACM0\n" |
||||
" -u <update_time_ms> Time in ms for uORB subscribed topics update. Default 0\n" |
||||
" -l <loops> How many iterations will this program have. -1 for infinite. Default 10000\n" |
||||
" -w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms\n" |
||||
" -b <baudrate> UART device baudrate. Default 460800\n" |
||||
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n" |
||||
" -r <reception port> UDP port for receiving. Default 2019\n" |
||||
" -s <sending port> UDP port for sending. Default 2020\n", |
||||
name); |
||||
} |
||||
|
||||
static int parse_options(int argc, char *argv[]) |
||||
{ |
||||
int ch; |
||||
int myoptind = 1; |
||||
const char *myoptarg = nullptr; |
||||
|
||||
while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF) |
||||
{ |
||||
switch (ch) |
||||
{ |
||||
case 't': _options.transport = strcmp(myoptarg, "UDP") == 0? |
||||
options::eTransports::UDP |
||||
:options::eTransports::UART; break; |
||||
case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; |
||||
case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break; |
||||
case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; |
||||
case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break; |
||||
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; |
||||
case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break; |
||||
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; |
||||
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; |
||||
default: |
||||
usage(argv[1]); |
||||
return -1; |
||||
} |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
static int micrortps_start(int argc, char *argv[]) |
||||
{ |
||||
if (0 > parse_options(argc, argv)) |
||||
{ |
||||
printf("EXITING...\n"); |
||||
_rtps_task = -1; |
||||
return -1; |
||||
} |
||||
|
||||
switch (_options.transport) |
||||
{ |
||||
case options::eTransports::UART: |
||||
{ |
||||
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); |
||||
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", |
||||
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms); |
||||
} |
||||
break; |
||||
case options::eTransports::UDP: |
||||
{ |
||||
transport_node = new UDP_node(_options.recv_port, _options.send_port); |
||||
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", |
||||
_options.recv_port, _options.send_port, _options.sleep_ms); |
||||
} |
||||
break; |
||||
default: |
||||
_rtps_task = -1; |
||||
printf("EXITING...\n"); |
||||
return -1; |
||||
} |
||||
|
||||
if (0 > transport_node->init()) |
||||
{ |
||||
printf("EXITING...\n"); |
||||
_rtps_task = -1; |
||||
return -1; |
||||
} |
||||
|
||||
sleep(1); |
||||
|
||||
struct timespec begin; |
||||
int total_read = 0; |
||||
uint32_t received = 0; |
||||
int loop = 0; |
||||
micrortps_start_topics(begin, total_read, received, loop); |
||||
|
||||
struct timespec end; |
||||
px4_clock_gettime(CLOCK_REALTIME, &end); |
||||
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000); |
||||
printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", |
||||
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read/(1000*elapsed_secs)); |
||||
|
||||
delete transport_node; |
||||
transport_node = nullptr; |
||||
PX4_INFO("exiting"); |
||||
fflush(stdout); |
||||
|
||||
_rtps_task = -1; |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
int micrortps_client_main(int argc, char *argv[]) |
||||
{ |
||||
if (argc < 2) |
||||
{ |
||||
usage(argv[0]); |
||||
return -1; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "start")) |
||||
{ |
||||
if (_rtps_task != -1) |
||||
{ |
||||
PX4_INFO("Already running"); |
||||
return -1; |
||||
} |
||||
|
||||
_rtps_task = px4_task_spawn_cmd("rtps", |
||||
SCHED_DEFAULT, |
||||
SCHED_PRIORITY_DEFAULT, |
||||
4096, |
||||
(px4_main_t) micrortps_start, |
||||
(char *const *)argv); |
||||
|
||||
if (_rtps_task < 0) |
||||
{ |
||||
PX4_WARN("Could not start task"); |
||||
_rtps_task = -1; |
||||
return -1; |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "stop")) |
||||
{ |
||||
if (_rtps_task == -1) |
||||
{ |
||||
PX4_INFO("Not running"); |
||||
return -1; |
||||
} |
||||
|
||||
_should_exit_task = true; |
||||
if (nullptr != transport_node) transport_node->close(); |
||||
return 0; |
||||
} |
||||
|
||||
usage(argv[0]); |
||||
|
||||
return -1; |
||||
} |
||||
Loading…
Reference in new issue