2 Copyright (C) 2012 Intel Corporation
4 This library is free software; you can redistribute it and/or
5 modify it under the terms of the GNU Lesser General Public
6 License as published by the Free Software Foundation; either
7 version 2.1 of the License, or (at your option) any later version.
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 Lesser General Public License for more details.
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19 #include "wheelplugin.h"
23 #include <boost/assert.hpp>
26 #include <gio/gunixinputstream.h>
27 #include <sys/types.h>
30 #include <linux/input.h>
31 #include <linux/joystick.h>
36 #include "timestamp.h"
39 #define LG27 "G27 Racing Wheel"
42 double gearRatio[8] = {
58 WheelPrivate(WheelSourcePlugin *parent, AbstractRoutingEngine *route);
61 AbstractPropertyType *getProperty(VehicleProperty::Property propType);
63 friend void readCallback(GObject *srcObj, GAsyncResult *res, gpointer userData);
67 void gotData(GAsyncResult *res);
68 void newButtonValue(char number, bool val);
69 void newAxisValue(char number, int val);
71 void changeMachineGuns(bool val);
72 void changeTurnSignal(TurnSignals::TurnSignalType dir, bool val);
73 void changeGear(int gear);
74 void changeOilPressure(bool increase);
75 void changeCoolantTemp(bool increase);
77 void changeSteeringAngle(int val);
78 void changeClutch(int val);
79 void changeThrottle(int val);
80 void changeBrake(int val);
82 uint16_t calcCarSpeed();
84 void checkButtonEvents();
86 AbstractRoutingEngine *re;
88 struct js_event jsEvent;
89 VehicleProperty::Property btnMap[16];
93 VehicleProperty::MachineGunTurretStatusType* machineGuns;
94 VehicleProperty::VehicleSpeedType *vehicleSpeed;
95 VehicleProperty::EngineSpeedType *engineSpeed;
96 VehicleProperty::TurnSignalType *turnSignal;
97 VehicleProperty::TransmissionShiftPositionType *shiftPosition;
98 VehicleProperty::TransmissionGearPositionType *gearPosition;
99 VehicleProperty::EngineOilPressureType *oilPSI;
100 VehicleProperty::EngineCoolantTemperatureType *coolantTemp;
101 VehicleProperty::SteeringWheelAngleType *steeringAngle;
102 VehicleProperty::SteeringWheelAngleW3CType *steeringAngleW3C;
103 VehicleProperty::ThrottlePositionType *throttle;
104 VehicleProperty::ClutchStatusType *clutch;
105 VehicleProperty::WheelBrakeType *brake;
107 AbstractSource* mParent;
109 VehicleProperty::ButtonEventType *tempButton;
113 WheelSourcePlugin::WheelSourcePlugin(AbstractRoutingEngine* re, map<string, string> config)
114 :AbstractSource(re, config)
116 this->mWheel = new WheelPrivate(this, re);
119 WheelSourcePlugin::~WheelSourcePlugin()
126 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
128 return new WheelSourcePlugin(routingengine, config);
131 const string WheelSourcePlugin::uuid()
133 return "c0ffee8a-c605-4a06-9034-59c1deadbeef";
136 void WheelSourcePlugin::getPropertyAsync(AsyncPropertyReply *reply)
138 reply->value = this->mWheel->getProperty(reply->property);
141 reply->success = true;
143 reply->completed(reply);
146 AsyncPropertyReply *WheelSourcePlugin::setProperty(AsyncSetPropertyRequest request )
151 void WheelSourcePlugin::subscribeToPropertyChanges(VehicleProperty::Property property)
153 mRequests.insert(property);
156 PropertyList WheelSourcePlugin::supported()
159 props.push_back(VehicleProperty::EngineSpeed);
160 props.push_back(VehicleProperty::VehicleSpeed);
161 props.push_back(VehicleProperty::TransmissionShiftPosition);
162 props.push_back(VehicleProperty::TransmissionGearPosition);
163 props.push_back(VehicleProperty::ThrottlePosition);
164 props.push_back(VehicleProperty::WheelBrake);
165 props.push_back(VehicleProperty::SteeringWheelAngle);
166 props.push_back(VehicleProperty::SteeringWheelAngleW3C);
167 props.push_back(VehicleProperty::TurnSignal);
168 props.push_back(VehicleProperty::ClutchStatus);
169 props.push_back(VehicleProperty::EngineOilPressure);
170 props.push_back(VehicleProperty::EngineCoolantTemperature);
171 props.push_back(VehicleProperty::MachineGunTurretStatus);
172 props.push_back(VehicleProperty::ButtonEvent);
177 int WheelSourcePlugin::supportedOperations()
182 PropertyInfo WheelSourcePlugin::getPropertyInfo(const VehicleProperty::Property &property)
184 Zone::ZoneList zones;
186 return PropertyInfo(0, zones);
189 void WheelSourcePlugin::unsubscribeToPropertyChanges(VehicleProperty::Property property)
191 mRequests.erase(property);
197 void readCallback(GObject *srcObj, GAsyncResult *res, gpointer userData)
200 throw std::runtime_error("Got a null WheelPrivate in the Read Callback!");
203 WheelPrivate *wp = (WheelPrivate *)userData;
208 WheelPrivate::WheelPrivate(WheelSourcePlugin *parent, AbstractRoutingEngine *route)
209 :re(route), gis(nullptr), axis(nullptr), button(nullptr),
210 oilPSI(new VehicleProperty::EngineOilPressureType(10)),
211 coolantTemp(new VehicleProperty::EngineCoolantTemperatureType(100)),
212 turnSignal(new VehicleProperty::TurnSignalType(TurnSignals::Off)),
213 throttle(new VehicleProperty::ThrottlePositionType(0)),
214 machineGuns(new VehicleProperty::MachineGunTurretStatusType(false)),
215 gearPosition(new VehicleProperty::TransmissionGearPositionType(Transmission::Neutral)),
216 shiftPosition(new VehicleProperty::TransmissionShiftPositionType(Transmission::Neutral)),
217 engineSpeed(new VehicleProperty::EngineSpeedType(0)),
218 vehicleSpeed(new VehicleProperty::VehicleSpeedType(0)),
219 steeringAngle(new VehicleProperty::SteeringWheelAngleType(0)),
220 steeringAngleW3C(new VehicleProperty::SteeringWheelAngleW3CType(0)),
221 clutch(new VehicleProperty::ClutchStatusType(false)),
222 brake(new VehicleProperty::WheelBrakeType(false)),
223 tempButton(new VehicleProperty::ButtonEventType(ButtonEvents::NoButton)),
227 unsigned char numAxes = 0;
228 unsigned char numButtons = 0;
231 char name[JSNAMELEN] = "Unknown";
232 struct js_corr cal[6];
234 //FIXME: Ugly as all get-out, but gets the job done quick...
235 unsigned int calData[36] = {
236 1, 0, 8191, 8192, 65542, 65534,
237 1, 0, 127, 128, 4227201, 4194176,
238 1, 0, 127, 128, 4227201, 4194176,
239 1, 0, 127, 128, 4227201, 4194176,
240 1, 0, 0, 0, 536854528, 536854528,
241 1, 0, 0, 0, 536854528, 536854528
245 //FIXME: Support config file with joystick device mapping, button/axis mappings, etc.
246 std::string jsdev = parent->getConfiguration()["device"];
248 DebugOut(0)<<"JSDev: "<<jsdev<<endl;
251 jsdev = "/dev/input/js0";
253 if ((fd = open(jsdev.c_str(), O_RDONLY)) < 0) {
254 throw std::runtime_error("Could not find a joystick class device!"); //FIXME: Later, don't throw, watch input devices, and jump on to any JS devices that appear
258 ioctl(fd, JSIOCGVERSION, &version);
259 ioctl(fd, JSIOCGAXES, &numAxes);
260 ioctl(fd, JSIOCGBUTTONS, &numButtons);
261 ioctl(fd, JSIOCGNAME(JSNAMELEN), name);
263 for (i = 0; i < 6; i++) {
266 cal[i].type = calData[(i*6)+k];
268 cal[i].prec = calData[(i*6)+k];
271 for(j = 0; j < 4; j++) {
272 cal[i].coef[j] = calData[(i*6)+k];
276 if (ioctl(fd, JSIOCSCORR, &cal) < 0) {
277 throw std::runtime_error("Could not set calibration data!");
281 DebugOut() << "Driver version: " << (version >> 16) << "." << ((version >> 8) & 0xFF) << "." << (version & 0xFF) << endl;
282 DebugOut() << "JS Name: " << name << endl;
283 DebugOut() << "JS Axes/Buttons: " << (int)numAxes << "/" << (int)numButtons << endl;
284 DebugOut() << "Converting FD to GIO Input Stream..." << endl;
285 this->axis = (int *)calloc(numAxes, sizeof(int));
286 this->button = (char *)calloc(numButtons, sizeof(char));
287 this->gis = g_unix_input_stream_new(fd, TRUE);
288 g_input_stream_read_async(this->gis, &this->jsEvent, sizeof(struct js_event), G_PRIORITY_DEFAULT, nullptr, &readCallback, this);
292 WheelPrivate::~WheelPrivate()
295 g_input_stream_close_async(this->gis, G_PRIORITY_DEFAULT, NULL, NULL, NULL);
299 AbstractPropertyType *WheelPrivate::getProperty(VehicleProperty::Property propType)
301 if (propType == VehicleProperty::VehicleSpeed)
303 vehicleSpeed->setValue(this->calcCarSpeed());
306 else if (propType == VehicleProperty::EngineSpeed)
308 engineSpeed->setValue(this->calcRPM());
311 else if (propType == VehicleProperty::TransmissionShiftPosition)
312 return this->shiftPosition;
313 else if (propType == VehicleProperty::TransmissionGearPosition)
314 return this->gearPosition;
315 else if (propType == VehicleProperty::ThrottlePosition)
316 return this->throttle;
317 else if (propType == VehicleProperty::WheelBrake)
319 else if (propType == VehicleProperty::SteeringWheelAngle)
320 return this->steeringAngle;
321 else if (propType == VehicleProperty::SteeringWheelAngleW3C)
322 return this->steeringAngleW3C;
323 else if (propType == VehicleProperty::TurnSignal)
324 return this->turnSignal;
325 else if (propType == VehicleProperty::ClutchStatus)
327 else if (propType == VehicleProperty::EngineOilPressure)
329 else if (propType == VehicleProperty::EngineCoolantTemperature)
330 return this->coolantTemp;
331 else if (propType == VehicleProperty::MachineGunTurretStatus)
332 return this->machineGuns;
333 else if (propType == VehicleProperty::ButtonEvent)
336 cout << "Unhandled getProperty type: " << propType << endl;
341 void WheelPrivate::newButtonValue(char number, bool val)
344 case 0: //Gear attach diamond down
347 case 1: //Gear attach diamond left
350 case 2: //Gear attach diamond right
353 case 3: //Gear attach diamond up
356 case 11://Gear attach red button row, left button
359 case 8: //Gear attach red button row, 2nd btn from left
362 case 9: //Gear attach red button row, 3rd btn from left
365 case 10://Gear attach red button row, 4th btn from left (right button)
368 case 4: //Right paddle shifter
369 if(val && this->gearPosition->basicValue() < MAX_GEARS)
371 this->changeGear(Transmission::TransmissionPositions(this->gearPosition->basicValue()+1));
372 changeMachineGuns(val);
375 case 5: //Left paddle shifter
376 if(val && this->gearPosition->basicValue() > 0)
377 this->changeGear(Transmission::TransmissionPositions(this->gearPosition->basicValue()-1));
379 case 6: //Right upper wheel button
380 this->changeTurnSignal(TurnSignals::Right, val);
382 case 18://Right middle wheel button
385 this->changeOilPressure(true);
387 case 19://Right lower wheel button
388 //Coolant temperature up
390 this->changeCoolantTemp(true);
392 case 7: //Left upper wheel button
393 this->changeTurnSignal(TurnSignals::Left, val);
395 case 20://Left middle wheel button
398 this->changeOilPressure(false);
400 case 21://Left lower wheel button
401 //Coolant temperature down
403 this->changeCoolantTemp(false);
406 this->changeGear((val ? 1 : 0));
409 this->changeGear((val ? 2 : 0));
412 this->changeGear((val ? 3 : 0));
415 this->changeGear((val ? 4 : 0));
418 this->changeGear((val ? 5 : 0));
421 this->changeGear((val ? 6 : 0));
423 case 22://Reverse gear
424 this->changeGear((val ? 128 : 0));
427 DebugOut() << "Got unknown button number: " << (int)number << endl;
432 void WheelPrivate::newAxisValue(char number, int val)
435 case 0: //Wheel angle, -32767 - 32767
436 //VehicleProperty::SteeringWheelAngle
437 this->changeSteeringAngle(val);
439 case 1: //Clutch, -32767 (depressed) - 32767 (undepressed)
440 this->changeClutch(val);
442 case 2: //Throttle, -32767 (depressed) - 32767 (undepressed)
443 //VehicleProperty::VehicleSpeed
444 //VehicleProperty::EngineSpeed
445 this->changeThrottle(val);
447 case 3: //Brake, -32767 (depressed) - 32767 (undepressed)
448 this->changeBrake(val);
450 case 4: //D-Pad L/R, -32767 (L), 0 (Released), 32767 (R)
452 case 5: //D-Pad U/D, -32767 (U), 0 (Released), 32767 (D)
455 cout << "Got unknown axis number: " << (int)number << endl;
460 void WheelPrivate::gotData(GAsyncResult *res)
462 GError *gerror = NULL;
463 int size = g_input_stream_read_finish(this->gis, res, &gerror);
466 throw std::runtime_error(gerror->message);
467 g_error_free(gerror);
469 if (size != sizeof(struct js_event)) {
470 cout << "Only read " << size << " bytes from js device - should have been " << sizeof(struct js_event) << " bytes!";
471 throw std::runtime_error("Bad read from JS device!");
475 switch (this->jsEvent.type & ~JS_EVENT_INIT) {
476 case JS_EVENT_BUTTON:
477 this->button[this->jsEvent.number] = this->jsEvent.value;
478 // cout << "Got button event, btn# " << (int)this->jsEvent.number << ", val " << this->jsEvent.value << endl;
479 this->newButtonValue(this->jsEvent.number, this->jsEvent.value);
482 this->axis[this->jsEvent.number] = this->jsEvent.value;
483 // cout << "Got axis event, axis# " << (int)this->jsEvent.number << ", val " << this->jsEvent.value << endl;
484 this->newAxisValue(this->jsEvent.number, this->jsEvent.value);
487 cout << "Got JS event that wasn't button or axis!" << endl;
492 g_input_stream_read_async(this->gis, &this->jsEvent, sizeof(struct js_event), G_PRIORITY_DEFAULT, nullptr, &readCallback, this);
496 //Data handling functions
499 void WheelPrivate::changeMachineGuns(bool val)
501 *(this->machineGuns) = val;
502 this->re->updateProperty(this->machineGuns, mParent->uuid());
505 void WheelPrivate::changeTurnSignal(TurnSignals::TurnSignalType dir, bool val)
507 TurnSignals::TurnSignalType tsVal= TurnSignals::Off;
509 if (dir == TurnSignals::Left)
510 tsVal = TurnSignals::Left;
512 tsVal = TurnSignals::Right;
514 *(this->turnSignal) = tsVal;
515 this->re->updateProperty(this->turnSignal, mParent->uuid());
518 void WheelPrivate::changeGear(int gear)
520 gearPosition->setValue((Transmission::TransmissionPositions)gear);
521 shiftPosition->setValue((Transmission::TransmissionPositions)gear);
523 vehicleSpeed->setValue(this->calcCarSpeed());
525 this->re->updateProperty(gearPosition, mParent->uuid());
526 this->re->updateProperty(shiftPosition, mParent->uuid());
527 this->re->updateProperty(vehicleSpeed, mParent->uuid());
530 void WheelPrivate::changeOilPressure(bool increase)
533 oilPSI->setValue(oilPSI->basicValue()+1);
534 else if(oilPSI->basicValue() > 0)
535 oilPSI->setValue(oilPSI->basicValue()-1);
537 this->re->updateProperty(oilPSI, mParent->uuid());
540 void WheelPrivate::changeCoolantTemp(bool increase)
542 (increase ? ++(*coolantTemp) : --(*coolantTemp));
544 this->re->updateProperty(coolantTemp, mParent->uuid());
548 void WheelPrivate::changeSteeringAngle(int val)
550 *steeringAngle = (((double)val/(double)32767.0) + (double)1.0) * (double)180.0;
551 *steeringAngleW3C = (((double)val/(double)32767.0) + (double)1.0) * (double)180.0;
552 this->re->updateProperty(steeringAngle, mParent->uuid());
553 this->re->updateProperty(steeringAngleW3C, mParent->uuid());
556 void WheelPrivate::changeClutch(int val)
559 *clutch = (val < 20000);
560 this->re->updateProperty(clutch, mParent->uuid());
564 void WheelPrivate::changeThrottle(int val)
566 *throttle = ((double)(val - 32767)/(double)-65534.0)*(double)100.0;
567 *vehicleSpeed = calcCarSpeed();
568 *engineSpeed = calcRPM();
570 this->re->updateProperty(throttle, mParent->uuid());
571 this->re->updateProperty(engineSpeed, mParent->uuid());
572 this->re->updateProperty(vehicleSpeed, mParent->uuid());
575 void WheelPrivate::changeBrake(int val)
577 *brake = (val < 20000);
578 this->re->updateProperty(brake, mParent->uuid());
582 uint16_t WheelPrivate::calcCarSpeed()
584 // cout << "Calc Car Speed, rpm: " << this->calcRPM() << ", gearRatio: " << gearRatio[this->currentGear == 128 ? 7 : this->currentGear] << " current gear: " << this->currentGear << endl;
585 return (this->calcRPM() * gearRatio[this->gearPosition->basicValue() == 128 ? 7 : this->gearPosition->basicValue()])/100;
589 uint16_t WheelPrivate::calcRPM()
591 // cout << "Calc rpm, throttle: " << this->throttle << endl;
592 return throttle->basicValue() * 100;
595 void WheelPrivate::checkButtonEvents()
597 if (this->button[0]) {
598 // cout << "Inside button 11!" << endl;
599 *tempButton = ButtonEvents::StopButton;
602 if (this->button[1]) {
603 // cout << "Inside button 11!" << endl;
604 *tempButton = (ButtonEvents::PrevButton);
606 if (this->button[2]) {
607 *tempButton = (ButtonEvents::SkipButton);
610 if (this->button[3]) {
611 *tempButton = ButtonEvents::PlayButton;
614 if (this->button[11]) {
615 *tempButton = (ButtonEvents::Preset1Button);
617 if (this->button[8]) {
618 *tempButton = (ButtonEvents::Preset2Button);
620 if (this->button[9]) {
621 *tempButton = (ButtonEvents::Preset3Button);
624 if (this->button[10]) {
625 *tempButton = (ButtonEvents::Preset4Button);
628 this->re->updateProperty(tempButton, mParent->uuid());