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

Simulator: use template for Reports

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-04-20 11:37:50 -07:00
parent 2f5bfe0c16
commit 88dc6ec1e5
2 changed files with 81 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.cpp
+11 -58
View File
@@ -46,59 +46,12 @@
#include <netinet/in.h>
#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);
src/modules/simulator/simulator.h
+70 -67
View File
@@ -40,83 +40,85 @@
#include <semaphore.h>
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 <typename RType> 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<simulator::RawAccelData> _accel;
simulator::Report<simulator::RawMPUData> _mpu;
simulator::Report<simulator::RawBaroData> _baro;
};