reverted varianttype
[profile/ivi/automotive-message-broker.git] / plugins / wheel / wheelplugin.cpp
1 /*
2 Copyright (C) 2012 Intel Corporation
3
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.
8
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.
13
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
17 */
18
19 #include "wheelplugin.h"
20
21 #include <iostream>
22 #include <stdexcept>
23 #include <boost/assert.hpp>
24 #include <glib.h>
25
26 #include <gio/gunixinputstream.h>
27 #include <sys/types.h>
28 #include <sys/stat.h>
29 #include <fcntl.h>
30 #include <linux/input.h>
31 #include <linux/joystick.h>
32
33 using namespace std;
34
35 #include "debugout.h"
36 #include "timestamp.h"
37
38 #define JSNAMELEN 128
39 #define LG27 "G27 Racing Wheel"
40 #define MAX_GEARS 6
41
42 double gearRatio[8] = {
43         0.0,    //Neutral
44         1.0/4.12,       //First
45         1.0/2.84,       //Second
46         1.0/2.28,       //Third
47         1.0/1.45,       //Fourth
48         1.0/1.0,        //Fifth
49         1.0/0.69,       //Sixth
50         1.0/3.21        //Reverse
51 };
52
53
54 class WheelPrivate
55 {
56
57 public:
58         WheelPrivate(WheelSourcePlugin *parent, AbstractRoutingEngine *route);
59         ~WheelPrivate();
60
61         AbstractPropertyType *getProperty(VehicleProperty::Property propType);
62
63         friend void readCallback(GObject *srcObj, GAsyncResult *res, gpointer userData);
64
65
66 private:
67         void gotData(GAsyncResult *res);
68         void newButtonValue(char number, bool val);
69         void newAxisValue(char number, int val);
70
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);
76
77         void changeSteeringAngle(int val);
78         void changeClutch(int val);
79         void changeThrottle(int val);
80         void changeBrake(int val);
81
82         uint16_t calcCarSpeed();
83         uint16_t calcRPM();
84         void checkButtonEvents();
85
86         AbstractRoutingEngine *re;
87         GInputStream *gis;
88         struct js_event jsEvent;
89         VehicleProperty::Property btnMap[16];
90         int *axis;
91         char *button;
92
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;
106
107         AbstractSource* mParent;
108
109         VehicleProperty::ButtonEventType *tempButton;
110 };
111
112
113 WheelSourcePlugin::WheelSourcePlugin(AbstractRoutingEngine* re, map<string, string> config)
114 :AbstractSource(re, config)
115 {
116         this->mWheel = new WheelPrivate(this, re);
117 }
118
119 WheelSourcePlugin::~WheelSourcePlugin()
120 {
121         delete this->mWheel;
122 }
123
124
125
126 extern "C" void create(AbstractRoutingEngine* routingengine, map<string, string> config)
127 {
128         new WheelSourcePlugin(routingengine, config);
129 }
130
131 const string WheelSourcePlugin::uuid()
132 {
133         return "c0ffee8a-c605-4a06-9034-59c1deadbeef";
134 }
135
136 void WheelSourcePlugin::getPropertyAsync(AsyncPropertyReply *reply)
137 {
138         reply->value = this->mWheel->getProperty(reply->property);
139
140         if(reply->value)
141                 reply->success = true;
142
143         reply->completed(reply);
144 }
145
146 AsyncPropertyReply *WheelSourcePlugin::setProperty(AsyncSetPropertyRequest request )
147 {
148
149 }
150
151 void WheelSourcePlugin::subscribeToPropertyChanges(VehicleProperty::Property property)
152 {
153         mRequests.insert(property);
154 }
155
156 PropertyList WheelSourcePlugin::supported()
157 {
158         PropertyList props;
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);
173
174         return props;
175 }
176
177 int WheelSourcePlugin::supportedOperations()
178 {
179         return Get | Set;
180 }
181
182 PropertyInfo WheelSourcePlugin::getPropertyInfo(const VehicleProperty::Property &property)
183 {
184         Zone::ZoneList zones;
185         zones.push_back(0);
186         return PropertyInfo(0, zones);
187 }
188
189 void WheelSourcePlugin::unsubscribeToPropertyChanges(VehicleProperty::Property property)
190 {
191         mRequests.erase(property);
192 }
193
194 //PIMPL:
195
196
197 void readCallback(GObject *srcObj, GAsyncResult *res, gpointer userData)
198 {
199         if (!userData) {
200                 throw std::runtime_error("Got a null WheelPrivate in the Read Callback!");
201         }
202
203         WheelPrivate *wp = (WheelPrivate *)userData;
204         wp->gotData(res);
205
206 }
207
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)),
224           mParent(parent)
225 {
226
227         unsigned char numAxes = 0;
228         unsigned char numButtons = 0;
229         int version = 0;
230         int fd;
231         char name[JSNAMELEN] = "Unknown";
232         struct js_corr cal[6];
233         int i, j;
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
242         };
243
244
245         //FIXME: Support config file with joystick device mapping, button/axis mappings, etc.
246         std::string jsdev = parent->getConfiguration()["device"];
247
248         DebugOut(0)<<"JSDev: "<<jsdev<<endl;
249
250         if(jsdev == "")
251                 jsdev = "/dev/input/js0";
252
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
255                 return;
256         }
257
258         ioctl(fd, JSIOCGVERSION, &version);
259         ioctl(fd, JSIOCGAXES, &numAxes);
260         ioctl(fd, JSIOCGBUTTONS, &numButtons);
261         ioctl(fd, JSIOCGNAME(JSNAMELEN), name);
262
263         for (i = 0; i < 6; i++) {
264                 int k = 0;
265
266                                 cal[i].type = calData[(i*6)+k];
267                 k++;
268                                 cal[i].prec = calData[(i*6)+k];
269                 k++;
270
271                                 for(j = 0; j < 4; j++) {
272                         cal[i].coef[j] = calData[(i*6)+k];
273                         k++;
274                                 }
275                 }
276         if (ioctl(fd, JSIOCSCORR, &cal) < 0) {
277                 throw std::runtime_error("Could not set calibration data!");
278                 return;
279         }
280
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);
289
290 }
291
292 WheelPrivate::~WheelPrivate()
293 {
294         if (this->gis)
295                 g_input_stream_close_async(this->gis, G_PRIORITY_DEFAULT, NULL, NULL, NULL);
296 }
297
298
299 AbstractPropertyType *WheelPrivate::getProperty(VehicleProperty::Property propType)
300 {
301         if (propType == VehicleProperty::VehicleSpeed)
302         {
303                 vehicleSpeed->setValue(this->calcCarSpeed());
304                 return vehicleSpeed;
305         }
306         else if (propType == VehicleProperty::EngineSpeed)
307         {
308                 engineSpeed->setValue(this->calcRPM());
309                 return engineSpeed;
310         }
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)
318                 return this->brake;
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)
326                 return this->clutch;
327         else if (propType == VehicleProperty::EngineOilPressure)
328                 return this->oilPSI;
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)
334                 return tempButton;
335         else
336                 cout << "Unhandled getProperty type: " << propType << endl;
337
338         return nullptr;
339 }
340
341 void WheelPrivate::newButtonValue(char number, bool val)
342 {
343         switch (number) {
344                 case 0: //Gear attach diamond down
345                         checkButtonEvents();
346                         break;
347                 case 1: //Gear attach diamond left
348                         checkButtonEvents();
349                         break;
350                 case 2: //Gear attach diamond right
351                         checkButtonEvents();
352                         break;
353                 case 3: //Gear attach diamond up
354                         checkButtonEvents();
355                         break;
356                 case 11://Gear attach red button row, left button
357                         checkButtonEvents();
358                         break;
359                 case 8: //Gear attach red button row, 2nd btn from left
360                         checkButtonEvents();
361                         break;
362                 case 9: //Gear attach red button row, 3rd btn from left
363                         checkButtonEvents();
364                         break;
365                 case 10://Gear attach red button row, 4th btn from left (right button)
366                         checkButtonEvents();
367                         break;
368                 case 4: //Right paddle shifter
369                         if(val && this->gearPosition->basicValue() < MAX_GEARS)
370                         {
371                                 this->changeGear(Transmission::TransmissionPositions(this->gearPosition->basicValue()+1));
372                                 changeMachineGuns(val);
373                         }
374                         break;
375                 case 5: //Left paddle shifter
376                         if(val && this->gearPosition->basicValue() > 0)
377                                 this->changeGear(Transmission::TransmissionPositions(this->gearPosition->basicValue()-1));
378                         break;
379                 case 6: //Right upper wheel button
380                         this->changeTurnSignal(TurnSignals::Right, val);
381                         break;
382                 case 18://Right middle wheel button
383                         //Oil pressure up
384                         if (val)
385                                 this->changeOilPressure(true);
386                         break;
387                 case 19://Right lower wheel button
388                         //Coolant temperature up
389                         if (val)
390                                 this->changeCoolantTemp(true);
391                         break;
392                 case 7: //Left upper wheel button
393                         this->changeTurnSignal(TurnSignals::Left, val);
394                         break;
395                 case 20://Left middle wheel button
396                         //Oil pressure down
397                         if (val)
398                                 this->changeOilPressure(false);
399                         break;
400                 case 21://Left lower wheel button
401                         //Coolant temperature down
402                         if (val)
403                                 this->changeCoolantTemp(false);
404                         break;
405                 case 12://1st gear
406                         this->changeGear((val ? 1 : 0));
407                         break;
408                 case 13://2nd gear
409                         this->changeGear((val ? 2 : 0));
410                         break;
411                 case 14://3rd gear
412                         this->changeGear((val ? 3 : 0));
413                         break;
414                 case 15://4th gear
415                         this->changeGear((val ? 4 : 0));
416                         break;
417                 case 16://5th gear
418                         this->changeGear((val ? 5 : 0));
419                         break;
420                 case 17://6th gear
421                         this->changeGear((val ? 6 : 0));
422                         break;
423                 case 22://Reverse gear
424                         this->changeGear((val ? 128 : 0));
425                         break;
426                 default:
427                         DebugOut() << "Got unknown button number: " << (int)number << endl;
428                         break;
429         }
430 }
431
432 void WheelPrivate::newAxisValue(char number, int val)
433 {
434         switch (number) {
435                 case 0: //Wheel angle, -32767 - 32767
436                         //VehicleProperty::SteeringWheelAngle
437                         this->changeSteeringAngle(val);
438                         break;
439                 case 1: //Clutch, -32767 (depressed) - 32767 (undepressed)
440                         this->changeClutch(val);
441                         break;
442                 case 2: //Throttle, -32767 (depressed) - 32767 (undepressed)
443                         //VehicleProperty::VehicleSpeed
444                         //VehicleProperty::EngineSpeed
445                         this->changeThrottle(val);
446                         break;
447                 case 3: //Brake, -32767 (depressed) - 32767 (undepressed)
448                         this->changeBrake(val);
449                         break;
450                 case 4: //D-Pad L/R, -32767 (L), 0 (Released), 32767 (R)
451                         break;
452                 case 5: //D-Pad U/D, -32767 (U), 0 (Released), 32767 (D)
453                         break;
454                 default:
455                         cout << "Got unknown axis number: " << (int)number << endl;
456                         break;
457         }
458 }
459
460 void WheelPrivate::gotData(GAsyncResult *res)
461 {
462         GError *gerror = NULL;
463         int size = g_input_stream_read_finish(this->gis, res, &gerror);
464
465         if (res < 0) {
466                 throw std::runtime_error(gerror->message);
467                 g_error_free(gerror);
468         }
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!");
472                 return;
473
474         } else {
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);
480                                 break;
481                         case JS_EVENT_AXIS:
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);
485                                 break;
486                         default:
487                                 cout << "Got JS event that wasn't button or axis!" << endl;
488                                 break;
489                 }
490         }
491
492         g_input_stream_read_async(this->gis, &this->jsEvent, sizeof(struct js_event), G_PRIORITY_DEFAULT, nullptr, &readCallback, this);
493 }
494
495
496 //Data handling functions
497
498
499 void WheelPrivate::changeMachineGuns(bool val)
500 {
501         *(this->machineGuns) = val;
502         this->re->updateProperty(this->machineGuns, mParent->uuid());
503 }
504
505 void WheelPrivate::changeTurnSignal(TurnSignals::TurnSignalType dir, bool val)
506 {
507         TurnSignals::TurnSignalType tsVal= TurnSignals::Off;
508         if (val) {
509                 if (dir == TurnSignals::Left)
510                         tsVal = TurnSignals::Left;
511                 else
512                         tsVal = TurnSignals::Right;
513         }
514         *(this->turnSignal) = tsVal;
515         this->re->updateProperty(this->turnSignal, mParent->uuid());
516 }
517
518 void WheelPrivate::changeGear(int gear)
519 {
520         gearPosition->setValue((Transmission::TransmissionPositions)gear);
521         shiftPosition->setValue((Transmission::TransmissionPositions)gear);
522
523         vehicleSpeed->setValue(this->calcCarSpeed());
524
525         this->re->updateProperty(gearPosition, mParent->uuid());
526         this->re->updateProperty(shiftPosition, mParent->uuid());
527         this->re->updateProperty(vehicleSpeed, mParent->uuid());
528 }
529
530 void WheelPrivate::changeOilPressure(bool increase)
531 {
532         if(increase)
533                 oilPSI->setValue(oilPSI->basicValue()+1);
534         else if(oilPSI->basicValue() > 0)
535                 oilPSI->setValue(oilPSI->basicValue()-1);
536
537         this->re->updateProperty(oilPSI, mParent->uuid());
538 }
539
540 void WheelPrivate::changeCoolantTemp(bool increase)
541 {
542         (increase ? ++(*coolantTemp) : --(*coolantTemp));
543
544         this->re->updateProperty(coolantTemp, mParent->uuid());
545 }
546
547
548 void WheelPrivate::changeSteeringAngle(int val)
549 {
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());
554 }
555
556 void WheelPrivate::changeClutch(int val)
557 {
558
559         *clutch = (val < 20000);
560         this->re->updateProperty(clutch, mParent->uuid());
561
562 }
563
564 void WheelPrivate::changeThrottle(int val)
565 {
566         *throttle = ((double)(val - 32767)/(double)-65534.0)*(double)100.0;
567         *vehicleSpeed = calcCarSpeed();
568         *engineSpeed = calcRPM();
569
570         this->re->updateProperty(throttle, mParent->uuid());
571         this->re->updateProperty(engineSpeed, mParent->uuid());
572         this->re->updateProperty(vehicleSpeed, mParent->uuid());
573 }
574
575 void WheelPrivate::changeBrake(int val)
576 {
577         *brake = (val < 20000);
578         this->re->updateProperty(brake, mParent->uuid());
579 }
580
581
582 uint16_t WheelPrivate::calcCarSpeed()
583 {
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;
586
587 }
588
589 uint16_t WheelPrivate::calcRPM()
590 {
591 //      cout << "Calc rpm, throttle: " << this->throttle << endl;
592         return throttle->basicValue() * 100;
593 }
594
595 void WheelPrivate::checkButtonEvents()
596 {
597         if (this->button[0]) {
598                 //      cout << "Inside button 11!" << endl;
599                 *tempButton = ButtonEvents::StopButton;
600
601         }
602         if (this->button[1]) {
603                 //      cout << "Inside button 11!" << endl;
604                 *tempButton = (ButtonEvents::PrevButton);
605         }
606         if (this->button[2]) {
607                 *tempButton = (ButtonEvents::SkipButton);
608
609         }
610         if (this->button[3]) {
611                 *tempButton = ButtonEvents::PlayButton;
612
613         }
614         if (this->button[11]) {
615                 *tempButton = (ButtonEvents::Preset1Button);
616         }
617         if (this->button[8]) {
618                 *tempButton = (ButtonEvents::Preset2Button);
619         }
620         if (this->button[9]) {
621                 *tempButton = (ButtonEvents::Preset3Button);
622
623         }
624         if (this->button[10]) {
625                 *tempButton = (ButtonEvents::Preset4Button);
626         }
627
628         this->re->updateProperty(tempButton, mParent->uuid());
629 }