From ac1679dbc3d8fae80b9eaeae2fb2a28c6d3cc675 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 14 Apr 2015 14:44:38 -0700 Subject: [PATCH] Added simulator Simulator listens for UDP input data at port 9876. Data is for now comma separated. Not yet connected to the various sim classes: accelsim, gyrosim, magsim. Barometer measurements not yet supported. Signed-off-by: Mark Charlebois --- src/modules/simulator/module.mk | 40 ++++++ src/modules/simulator/simulator.cpp | 189 ++++++++++++++++++++++++++++ src/modules/simulator/simulator.h | 86 +++++++++++++ 3 files changed, 315 insertions(+) create mode 100644 src/modules/simulator/module.mk create mode 100644 src/modules/simulator/simulator.cpp create mode 100644 src/modules/simulator/simulator.h diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk new file mode 100644 index 0000000000..483204d5e4 --- /dev/null +++ b/src/modules/simulator/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2015 MArk Charlebois. All rights reserved. +# +# 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 PX4 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 OWNER 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. +# +############################################################################ + +# +# Makefile to build simulator +# + +MODULE_COMMAND = simulator + +SRCS = simulator.cpp diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp new file mode 100644 index 0000000000..c4c6ffbbd6 --- /dev/null +++ b/src/modules/simulator/simulator.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * 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 PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file simulator.cpp + * A device simulator + */ + +#include +#include +#include +#include +#include +#include +#include "simulator.h" + +Simulator *Simulator::_instance = NULL; + +Simulator::Simulator() : _max_readers(3) +{ + sem_init(&_lock, 0, _max_readers); +} + +Simulator *Simulator::getInstance() +{ + return _instance; +} + +int Simulator::getSample(sim_dev_t dev, sample &val) +{ + int ret; + + switch (dev) { + case SIM_GYRO: + read_lock(); + val = _gyro[_readidx]; + read_unlock(); + ret = 0; + break; + case SIM_ACCEL: + read_lock(); + val = _accel[_readidx]; + read_unlock(); + ret = 0; + break; + case SIM_MAG: + read_lock(); + val = _mag[_readidx]; + read_unlock(); + ret = 0; + break; + default: + ret = 1; + } + return ret; +} + +int Simulator::start(int argc, char *argv[]) +{ + int ret = 0; + _instance = new Simulator(); + if (_instance) + _instance->updateSamples(); + else { + printf("Simulator creation failed\n"); + ret = 1; + } + return ret; +} + + +void Simulator::updateSamples() +{ + // get samples from external provider + struct sockaddr_in myaddr; + struct sockaddr_in srcaddr; + socklen_t addrlen = sizeof(srcaddr); + int len, fd; + const int buflen = 200; + const int port = 9876; + unsigned char buf[buflen]; + int writeidx, num; + + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + printf("create socket failed\n"); + return; + } + + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(port); + + if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + printf("bind failed\n"); + return; + } + + for (;;) { + len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); + if (len > 0) { + writeidx = !_readidx; + buf[len] = 0; + printf("received: %s\n", buf); + // FIXME - temp hack to read data, not safe - bad bad bad + num = sscanf((const char *)buf, "%f,%f,%f,%f,%f,%f,%f,%f,%f", + &_gyro[writeidx].x, &_gyro[writeidx].y, &_gyro[writeidx].z, + &_accel[writeidx].x, &_accel[writeidx].y, &_accel[writeidx].z, + &_mag[writeidx].x, &_mag[writeidx].y, &_mag[writeidx].z); + if (num != 9) { + printf("Only read %d items\n", num); + } + else { + // Swap read and write buffers + write_lock(); + _readidx = !_readidx; + write_unlock(); + } + } + } +} + +void Simulator::read_lock() +{ + sem_wait(&_lock); +} +void Simulator::read_unlock() +{ + sem_post(&_lock); +} +void Simulator::write_lock() +{ + for (int i=0; i<_max_readers; i++) { + sem_wait(&_lock); + } +} +void Simulator::write_unlock() +{ + for (int i=0; i<_max_readers; i++) { + sem_post(&_lock); + } +} + +extern "C" { + +int simulator_main(int argc, char *argv[]) +{ + return (int)(px4_task_spawn_cmd("Simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + Simulator::start, + nullptr) < 0); + + return 0; +} + +} + diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h new file mode 100644 index 0000000000..4c20768caf --- /dev/null +++ b/src/modules/simulator/simulator.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * 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 PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file simulator.h + * A device simulator + */ + +#pragma once + +#include + +class Simulator { +public: + static Simulator *getInstance(); + + enum sim_dev_t { + SIM_GYRO, + SIM_ACCEL, + SIM_MAG + }; + + struct sample { + float x; + float y; + float z; + sample() : x(0), y(0), z(0) {} + sample(float a, float b, float c) : x(a), y(b), z(c) {} + }; + + static int start(int argc, char *argv[]); + + int getSample(sim_dev_t dev, sample &val); +private: + Simulator(); + ~Simulator() { _instance=NULL; } + + void updateSamples(); + + void read_lock(); + void read_unlock(); + void write_lock(); + void write_unlock(); + + int _readidx; + + sample _accel[2]; + sample _gyro[2]; + sample _mag[2]; + + static Simulator *_instance; + sem_t _lock; + + const int _max_readers; +}; +