diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 3372f25f4b..12be371339 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -50,11 +50,55 @@ static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; -Simulator::Simulator() : _max_readers(3) +SimulatorReport::SimulatorReport(int readers, int reportLen) : + _max_readers(readers), + _report_len(reportLen) { sem_init(&_lock, 0, _max_readers); } +bool SimulatorReport::copyData(void *inbuf, void *outbuf, int len) +{ + if (len != _report_len) { + return false; + } + read_lock(); + memcpy(inbuf, outbuf, _report_len); + read_unlock(); + return true; +} + +bool SimulatorReport::writeData(void *inbuf, void *outbuf, int len) +{ + if (len != _report_len) { + return false; + } + memcpy(inbuf, outbuf, _report_len); + swapBuffers(); + return true; +} + +void SimulatorReport::read_lock() +{ + sem_wait(&_lock); +} +void SimulatorReport::read_unlock() +{ + sem_post(&_lock); +} +void SimulatorReport::write_lock() +{ + for (int i=0; i<_max_readers; i++) { + sem_wait(&_lock); + } +} +void SimulatorReport::write_unlock() +{ + for (int i=0; i<_max_readers; i++) { + sem_post(&_lock); + } +} + Simulator *Simulator::getInstance() { return _instance; @@ -62,13 +106,17 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { - if (len != sizeof(MPUReport)) { - return false; - } - read_lock(); - memcpy(buf, &_mpureport[_readidx], sizeof(MPUReport)); - read_unlock(); - return true; + return _mpu.copyData(buf, &_mpu._data[_mpu.getReadIdx()], len); +} + +bool Simulator::getRawAccelReport(uint8_t *buf, int len) +{ + return _accel.copyData(buf, &_accel._data[_accel.getReadIdx()], len); +} + +bool Simulator::getBaroSample(uint8_t *buf, int len) +{ + return _baro.copyData(buf, &_baro._data[_baro.getReadIdx()], len); } int Simulator::start(int argc, char *argv[]) @@ -95,7 +143,6 @@ void Simulator::updateSamples() const int buflen = 200; const int port = 9876; unsigned char buf[buflen]; - int writeidx; if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { printf("create socket failed\n"); @@ -115,15 +162,17 @@ void Simulator::updateSamples() for (;;) { len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { - writeidx = !_readidx; - if (len == sizeof(MPUReport)) { + if (len == sizeof(MPUReport::RawMPUData)) { printf("received: MPU data\n"); - memcpy((void *)&_mpureport[writeidx], (void *)buf, len); - - // Swap read and write buffers - write_lock(); - _readidx = !_readidx; - write_unlock(); + _mpu.writeData(&_mpu._data[_mpu.getWriteIdx()], buf, len); + } + else if (len == sizeof(RawAccelReport::RawAccelData)) { + printf("received: accel data\n"); + _accel.writeData(&_accel._data[_accel.getWriteIdx()], buf, len); + } + else if (len == sizeof(BaroReport::RawBaroData)) { + printf("received: accel data\n"); + _baro.writeData(&_baro._data[_baro.getWriteIdx()], buf, len); } else { printf("bad packet: len = %d\n", len); @@ -132,27 +181,6 @@ void Simulator::updateSamples() } } -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); - } -} - static void usage() { warnx("Usage: simulator {start|stop}"); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 333a729281..c9e8a73c97 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -40,6 +40,85 @@ #include +class SimulatorReport { +public: + SimulatorReport(int readers, int reportLen); + ~SimulatorReport() {}; + + int getReadIdx() { return _readidx; } + int getWriteIdx() { return !_readidx; } + + bool copyData(void *inbuf, void *outbuf, int len); + bool writeData(void *inbuf, void *outbuf, int len); + +protected: + void read_lock(); + void read_unlock(); + void write_lock(); + void write_unlock(); + + void swapBuffers() + { + write_lock(); + _readidx = !_readidx; + write_unlock(); + } + + int _readidx; + sem_t _lock; + const int _max_readers; + const int _report_len; +}; + +class RawAccelReport : public SimulatorReport { +public: + RawAccelReport() : SimulatorReport(1, sizeof(RawAccelData)) {} + ~RawAccelReport() {} + +// FIXME - what is the endianness of these on actual device? +#pragma pack(push, 1) + struct RawAccelData { + int16_t x; + int16_t y; + int16_t z; + }; +#pragma pack(pop) + + RawAccelData _data[2]; +}; + +class MPUReport : public SimulatorReport { +public: + MPUReport() : SimulatorReport(1, sizeof(RawMPUData)) {} + ~MPUReport() {} + +#pragma pack(push, 1) + struct RawMPUData { + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) + + RawMPUData _data[2]; +}; + +class BaroReport : public SimulatorReport { +public: + BaroReport() : SimulatorReport(1, sizeof(RawBaroData)) {} + ~BaroReport() {} + + struct RawBaroData { + uint8_t d[3]; + }; + + RawBaroData _data[2]; +}; + class Simulator { public: static Simulator *getInstance(); @@ -60,44 +139,18 @@ public: static int start(int argc, char *argv[]); + bool getRawAccelReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); + bool getBaroSample(uint8_t *buf, int len); private: - Simulator(); + 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; - -#pragma pack(push, 1) - /** - * Report conversation within the GYROSIM, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - }; -#pragma pack(pop) - MPUReport _mpureport[2]; + RawAccelReport _accel; + MPUReport _mpu; + BaroReport _baro; }; diff --git a/src/platforms/linux/drivers/barosim/baro_sim.cpp b/src/platforms/linux/drivers/barosim/baro_sim.cpp index 6e4e1ca0b0..a9374137fe 100644 --- a/src/platforms/linux/drivers/barosim/baro_sim.cpp +++ b/src/platforms/linux/drivers/barosim/baro_sim.cpp @@ -50,6 +50,7 @@ #include #include +#include #include "barosim.h" #include "board_config.h" @@ -201,5 +202,16 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, { // TODO add Simulation data connection so calls retrieve // data from the simulator + + if (send_len != 1 || send[0] != 0 || recv_len != 3) { + return 1; + } + else { + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { + return -ENODEV; + } + sim->getBaroSample(recv, recv_len); + } return 0; }