diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 12be371339..9984896525 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -46,59 +46,12 @@ #include #include "simulator.h" +using namespace simulator; + static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; -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; @@ -106,17 +59,17 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { - return _mpu.copyData(buf, &_mpu._data[_mpu.getReadIdx()], len); + return _mpu.copyData(buf, len); } bool Simulator::getRawAccelReport(uint8_t *buf, int len) { - return _accel.copyData(buf, &_accel._data[_accel.getReadIdx()], len); + return _accel.copyData(buf, len); } bool Simulator::getBaroSample(uint8_t *buf, int len) { - return _baro.copyData(buf, &_baro._data[_baro.getReadIdx()], len); + return _baro.copyData(buf, len); } int Simulator::start(int argc, char *argv[]) @@ -162,17 +115,17 @@ void Simulator::updateSamples() for (;;) { len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { - if (len == sizeof(MPUReport::RawMPUData)) { + if (len == sizeof(RawMPUData)) { printf("received: MPU data\n"); - _mpu.writeData(&_mpu._data[_mpu.getWriteIdx()], buf, len); + _mpu.writeData(buf); } - else if (len == sizeof(RawAccelReport::RawAccelData)) { + else if (len == sizeof(RawAccelData)) { printf("received: accel data\n"); - _accel.writeData(&_accel._data[_accel.getWriteIdx()], buf, len); + _accel.writeData(buf); } - else if (len == sizeof(BaroReport::RawBaroData)) { + else if (len == sizeof(RawBaroData)) { printf("received: accel data\n"); - _baro.writeData(&_baro._data[_baro.getWriteIdx()], buf, len); + _baro.writeData(buf); } else { printf("bad packet: len = %d\n", len); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index c9e8a73c97..e07cc0f296 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -40,83 +40,85 @@ #include -class SimulatorReport { +namespace simulator { + +// 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) + +#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) + +struct RawBaroData { + uint8_t d[3]; +}; + +template class Report { public: - SimulatorReport(int readers, int reportLen); - ~SimulatorReport() {}; + Report(int readers) : + _max_readers(readers), + _report_len(sizeof(RType)) + { + sem_init(&_lock, 0, _max_readers); + } - int getReadIdx() { return _readidx; } - int getWriteIdx() { return !_readidx; } + ~Report() {}; - bool copyData(void *inbuf, void *outbuf, int len); - bool writeData(void *inbuf, void *outbuf, int len); + bool copyData(void *outbuf, int len) + { + if (len != _report_len) { + return false; + } + read_lock(); + memcpy(outbuf, &_buf[_readidx], _report_len); + read_unlock(); + return true; + } + void writeData(void *inbuf) + { + write_lock(); + memcpy(&_buf[!_readidx], inbuf, _report_len); + _readidx = !_readidx; + write_unlock(); + } protected: - void read_lock(); - void read_unlock(); - void write_lock(); - void write_unlock(); - - void swapBuffers() + void read_lock() { sem_wait(&_lock); } + void read_unlock() { sem_post(&_lock); } + void write_lock() { - write_lock(); - _readidx = !_readidx; - write_unlock(); + for (int i=0; i<_max_readers; i++) { + sem_wait(&_lock); + } + } + void write_unlock() + { + for (int i=0; i<_max_readers; i++) { + sem_post(&_lock); + } } int _readidx; sem_t _lock; const int _max_readers; const int _report_len; + RType _buf[2]; }; -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 { @@ -143,14 +145,15 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); private: - Simulator() {} + Simulator() : _accel(1), _mpu(1), _baro(1) {} ~Simulator() { _instance=NULL; } void updateSamples(); static Simulator *_instance; - RawAccelReport _accel; - MPUReport _mpu; - BaroReport _baro; + + simulator::Report _accel; + simulator::Report _mpu; + simulator::Report _baro; };