yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Linux: Added barosim support to simulator

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-04-15 16:12:08 -07:00
parent 89bc1b86b2
commit 93c39e6d1d
3 changed files with 163 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
src/modules/simulator/simulator.cpp
View File

@@ -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}");