source: flair-src/branches/mavlink/lib/FlairSensorActuator/src/TargetController.cpp @ 46

Last change on this file since 46 was 42, checked in by Sanahuja Guillaume, 5 years ago

m

File size: 4.4 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:    2015/03/30
6//  filename:   TargetController.cpp
7//
8//  author:     Gildas Bayard
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Base class for target side remote controls
14//
15//
16/*********************************************************************/
17#include "TargetController.h"
18#include "Controller.h"
19#include <TabWidget.h>
20#include <Tab.h>
21#include <FrameworkManager.h>
22#include <cvmatrix.h>
23
24#include <cstring>
25#include <string>
26
27#include <unistd.h> //for usleep
28
29using namespace flair::core;
30using namespace flair::gui;
31using std::string;
32
33namespace flair {
34namespace sensor {
35
36TargetController::TargetController(const FrameworkManager* parent,string name,uint8_t priority) :
37    Thread(parent,name,priority),IODevice(parent,name) {
38    main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
39    TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
40    setup_tab=new Tab(tab,"Reglages");
41}
42
43TargetController::~TargetController() {
44  SafeStop();
45  Join();
46}
47
48std::string TargetController::GetAxisName(unsigned int axisId) const {
49  return string("axis") + std::to_string(axisId);
50}
51
52std::string TargetController::GetButtonName(unsigned int buttonId) const {
53  return string("button") + std::to_string(buttonId);
54}
55
56bool TargetController::SetLedOn(unsigned int ledId) {
57  if (!IsControllerActionSupported(ControllerAction::SetLedOn))
58    return false;
59  SwitchLedMessage *msgLed = new SwitchLedMessage(true, ledId);
60  changeStateQueue.push(msgLed);
61  return true;
62}
63
64bool TargetController::SetLedOff(unsigned int ledId) {
65  if (!IsControllerActionSupported(ControllerAction::SetLedOn))
66    return false;
67  SwitchLedMessage *msgLed = new SwitchLedMessage(false, ledId);
68  changeStateQueue.push(msgLed);
69  return true;
70}
71
72bool TargetController::Rumble(unsigned int leftForce, unsigned int leftTimeout,
73                              unsigned int rightForce,
74                              unsigned int rightTimeout) {
75  if (!IsControllerActionSupported(ControllerAction::Rumble))
76    return false;
77  RumbleMessage *msgRumble =
78      new RumbleMessage(leftForce, leftTimeout, rightForce, rightTimeout);
79  changeStateQueue.push(msgRumble);
80  return true;
81}
82
83bool TargetController::FlashLed(unsigned int ledId, unsigned int onTimeout,
84                                unsigned int offTimeout) {
85  if (!IsControllerActionSupported(ControllerAction::FlashLed))
86    return false;
87  FlashLedMessage *msgFlashLed =
88      new FlashLedMessage(ledId, onTimeout, offTimeout);
89  changeStateQueue.push(msgFlashLed);
90  return true;
91}
92
93float TargetController::GetAxisValue(unsigned int axisId) const {
94  // TODO we'd better throw an exception here
95  if (axis == NULL)
96    return 0;
97
98  axis->GetMutex();
99  float axisValue = axis->Value(axisId, 0);
100  axis->ReleaseMutex();
101  return axisValue;
102}
103
104bool TargetController::IsButtonPressed(unsigned int buttonId) const {
105  // TODO we'd better throw an exception here
106  if (button == NULL)
107    return false;
108
109  button->GetMutex();
110  bool buttonValue = button->Value(buttonId, 0);
111  button->ReleaseMutex();
112  return buttonValue;
113}
114
115void TargetController::Run() {
116  SetPeriodMS(20);; //50Hz
117
118  Message *message;
119
120  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
121    SafeStop();
122    Thread::Err("An error occured, we don't proceed with the loop.\n");
123  } else {
124
125    axis = new cvmatrix((IODevice *)this, axisNumber, 1, floatType);
126    button =
127        new cvmatrix((IODevice *)this, buttonNumber, 1, SignedIntegerType(8));
128
129    while (!ToBeStopped()) {
130      // Thread::Info("Debug: entering acquisition loop\n");
131      if (getFrameworkManager()->ConnectionLost() == true)
132        SafeStop();
133
134      if (IsDataFrameReady()) {
135        // Thread::Info("Debug: data frame is ready\n");
136
137        AcquireAxisData(*axis);
138        AcquireButtonData(*button);
139
140        // send the data
141        axis->SetDataTime(GetTime());
142        ProcessUpdate(axis);
143
144        // send pending controller state change request(s)
145
146        while (changeStateQueue.size() != 0) {
147          message = changeStateQueue.front();
148          if (ProcessMessage(message)) {
149            changeStateQueue.pop();
150            delete message;
151          }
152        }
153      }
154      WaitPeriod();
155    }
156  }
157}
158
159Tab *TargetController::GetTab() const { return setup_tab; }
160
161} // end namespace sensor
162} // end namespace flair
Note: See TracBrowser for help on using the repository browser.