source: flair-src/trunk/lib/FlairSensorActuator/src/SimulatedUgvControls.cpp @ 377

Last change on this file since 377 was 377, checked in by Sanahuja Guillaume, 10 months ago

ugv update

  • Property svn:eol-style set to native
File size: 1.9 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2020/&2/09
6//  filename:   SimulatedUgvControls.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class for a simulation ugv controls
14//
15//
16/*********************************************************************/
17#ifdef CORE2_64
18
19#include "SimulatedUgvControls.h"
20#include <FrameworkManager.h>
21#include <Matrix.h>
22#include <SharedMem.h>
23#include <sstream>
24#include <string.h>
25
26using std::string;
27using std::ostringstream;
28using namespace flair::core;
29
30namespace flair {
31namespace actuator {
32
33SimulatedUgvControls::SimulatedUgvControls(string name, uint32_t modelId,uint32_t deviceId)
34    : UgvControls(name) {
35 
36  shmem = new SharedMem(this, ShMemName(modelId, deviceId),2 * sizeof(float)+sizeof(Time));
37 
38  buf=(char*)malloc(2 * sizeof(float)+sizeof(Time));
39  if(buf==NULL) {
40    Err("error creating buffer\n");
41    return;
42  }
43 
44  SetIsReady(true);
45}
46
47SimulatedUgvControls::~SimulatedUgvControls() {
48  if(buf!=NULL) free(buf);
49}
50
51string SimulatedUgvControls::ShMemName(uint32_t modelId,uint32_t deviceId) {
52  ostringstream dev_name;
53  dev_name << "simu" <<  modelId << "_ugvcontrols_" << deviceId;
54  return dev_name.str().c_str();
55}
56
57void SimulatedUgvControls::SetControls(float speed,float turn) {
58  float *values=(float*)buf;
59  values[0]=speed;
60  values[1]=turn;
61  Time time=GetTime();
62  memcpy(buf+2 * sizeof(float),&time,sizeof(Time));
63
64  shmem->Write(buf, 2 * sizeof(float)+sizeof(Time));
65
66  // on prend une fois pour toute le mutex et on fait des accès directs
67  output->GetMutex();
68  for (int i = 0; i < 2; i++) {
69    output->SetValueNoMutex(i, 0, values[i]);
70  }
71  output->ReleaseMutex();
72}
73
74
75} // end namespace sensor
76} // end namespace flair
77
78#endif
Note: See TracBrowser for help on using the repository browser.