src/CAvgCar.cpp

   1 /*
   2  * Copyright (c) 2013, TOYOTA MOTOR CORPORATION.
   3  *
   4  * This program is licensed under the terms and conditions of the
   5  * Apache License, version 2.0.  The full text of the Apache License is at
   6  * http://www.apache.org/licenses/LICENSE-2.0
   7  *
   8  */
   9
  10 /**
  11  * @brief   virtual car
  12  *          RPM / Speed / Brake Value is determined by the average
  13  *
  14  * @date    Apr-08-2013 create
  15  */
  16
  17 #include <stdlib.h>
  18 #include <time.h>
  19 #include <memory.h>
  20 #include "CAvgCar.h"
  21
  22 /******************************************
  23  * average Machine
  24 ******************************************/
  25
  26 /**
  27  * @brief averageMachine
  28  *        Constructor
  29  * @param sz average sample space size
  30  */
  31 averageMachine::averageMachine(const short sz)
  32 {
  33     m_sz = sz;
  34     m_n = (double) sz;
  35     m_x = new double[sz];
  36     memset(m_x, 0, sizeof(double) * sz);
  37     m_posi = 0;
  38     m_prev = 0;
  39     m_t = 0.0;
  40     m_avg = 0.0;
  41 }
  42
  43 /**
  44  * @brief ~averageMachine
  45  *        destructor
  46  */
  47 averageMachine::~averageMachine()
  48 {
  49     delete[] m_x;
  50 }
  51
  52 /**
  53  * @brief setSample
  54  * @param x sample value
  55  */
  56 void averageMachine::setSample(double x)
  57 {
  58     short next = m_posi + 1;
  59     if (next >= m_sz) {
  60         next = 0;
  61     }
  62     m_x[m_posi] = x;            // store sample value
  63     m_t += x;                   // get sum
  64     m_t -= m_x[next];
  65     m_prev = m_posi;
  66     m_posi = next;              // next position
  67     m_avg = m_t / m_n;          // get average
  68 }
  69
  70 /**
  71  * @brief reCalc
  72  */
  73 void averageMachine::reCalc()
  74 {
  75     m_t = 0.0l;
  76     for (int i = 0; i < m_sz; i++) {
  77         m_t += m_x[i];
  78     }
  79     m_avg = m_t / m_n;
  80 }
  81
  82 /******************************************
  83  * average Machine Integer
  84 ******************************************/
  85
  86 /**
  87  * @brief iAverageMachine
  88  *        Constructor
  89  */
  90 iAverageMachine::iAverageMachine(const short sz)
  91 {
  92     m_sz = sz;
  93     m_n = (int) sz;
  94     m_x = new int[sz];
  95     memset(m_x, 0, sizeof(int) * sz);
  96     m_posi = 0;
  97     m_prev = 0;
  98     m_t = 0;
  99     m_avg = 0.0;
 100     m_iAvg = 0;
 101 }
 102
 103 /**
 104  * @brief ~iAverageMachine
 105  *       destructor
 106  */
 107 iAverageMachine::~iAverageMachine()
 108 {
 109     delete[] m_x;
 110 }
 111
 112 /**
 113  * @brief setSample
 114  * @param x sample value
 115  */
 116 void iAverageMachine::setSample(int x)
 117 {
 118     short next = m_posi + 1;
 119     if (next >= m_sz) {
 120         next = 0;
 121     }
 122     m_x[m_posi] = x;            // store sample value
 123     m_t += x;                   // get sum
 124     m_t -= m_x[next];
 125     m_prev = m_posi;
 126     m_posi = next;              // next position
 127     m_avg = (double) m_t / (double) m_n;        // get average
 128     m_iAvg = m_t / m_n;
 129 }
 130
 131 /**
 132  * @brief reCalc
 133  */
 134 void iAverageMachine::reCalc()
 135 {
 136     m_t = 0;
 137     for (int i = 0; i < m_sz; i++) {
 138         m_t += m_x[i];
 139     }
 140     m_avg = (double) m_t / (double) m_n;        // get average
 141     m_iAvg = m_t / m_n;
 142 }
 143
 144 /******************************************
 145  * Average Engne
 146 ******************************************/
 147
 148 /**
 149  * @brief CAvgEngine
 150  *        Constructor
 151  */
 152 CAvgEngine::CAvgEngine(const short sz)
 153     :m_avgRPM(sz? sz: D_SAMPLE_SPACE_RPM)
 154 {
 155     m_active = false;
 156     m_rpm = 0.0;
 157 }
 158
 159 /**
 160  * @brief ~CAvgEngine
 161  */
 162 CAvgEngine::~CAvgEngine()
 163 {
 164 }
 165
 166 /**
 167  * @brief startter
 168  */
 169 void CAvgEngine::ignitionStart(int rpm)
 170 {
 171     m_active = true;
 172     if (-1 != rpm) {
 173         m_rpm = (double) rpm;
 174     }
 175 }
 176
 177 /**
 178  * @brief chgThrottle
 179  *       Gets the RPM from the throttle
 180  * @param throttle 0 - 65535
 181  */
 182 void CAvgEngine::chgThrottle(int throttle)
 183 {
 184     double rpm = (((double) throttle) / 65534.0l) * 100.0l;
 185     if (false == isActive()) {
 186         return;
 187     }
 188     m_avgRPM.setSample(rpm);
 189     m_rpm = m_avgRPM.getAvg();
 190     if (D_RPM_BOTTOM_BORDER > m_rpm) {
 191         m_rpm = D_IDLING_RPM + ((double) rand() / RAND_MAX);
 192     }
 193 }
 194
 195 /******************************************
 196  * Average Brake
 197 ******************************************/
 198 /**
 199  * @brief CAvgBrake
 200  *        Constructor
 201  * @param sz sample space size
 202  */
 203 CAvgBrake::CAvgBrake(const short sz)
 204     :m_brk(sz? sz: D_SAMPLE_SPACE_BRAKE)
 205 {
 206     m_brakeVal = 0;
 207     m_brake = false;
 208 }
 209
 210 /**
 211  * @brief ~CAvgBrake
 212  *        destructor
 213  */
 214 CAvgBrake::~CAvgBrake()
 215 {
 216 }
 217
 218 /**
 219  * @brief chgBrake
 220  *       Gets the Brake Infomation from the brake value
 221  * @param brake 0 - 65535
 222  */
 223 void CAvgBrake::chgBrake(int brake)
 224 {
 225     m_brk.setSample(brake);
 226     m_brakeVal = m_brk.getIAvg();
 227     judgeBrake();
 228 }
 229
 230 /**
 231  * @brief judgeBreak
 232  */
 233 void CAvgBrake::judgeBrake()
 234 {
 235     m_brake = false;
 236     if (D_LAG_BRAKE < m_brakeVal) {
 237         m_brake = true;
 238     }
 239 }
 240
 241 /**
 242  * @brief getSpeed
 243  * @param sourceSpeed speed
 244  * @return Calculate the load on the brake
 245  */
 246 #define D_SOFT_SPEED_STOP   15.0l
 247 #define D_MIDDLE_SPEED_STOP 22.0l
 248 #define D_HARD_SPEED_STOP   30.0l
 249
 250 /* SOFT */
 251 #define D_S_SPEED    30
 252 #define D_S_SPEED_S  0.4
 253 #define D_S_SPEED_M  0.6
 254 #define D_S_SPEED_H  0.8
 255
 256 /* MIDDLE */
 257 #define D_M_SPEED    60
 258 #define D_M_SPEED_S  0.3
 259 #define D_M_SPEED_M  0.5
 260 #define D_M_SPEED_H  0.7
 261
 262 /* HARD */
 263 #define D_H_SPEED_S  0.1
 264 #define D_H_SPEED_M  0.3
 265 #define D_H_SPEED_H  0.5
 266
 267 double CAvgBrake::getSpeed(double sourceSpeed)
 268 {
 269     if (false == m_brake) {
 270         return sourceSpeed;
 271     }
 272
 273     if (D_SOFT_BRAKE_MAX > m_brakeVal) {
 274         if (D_SOFT_SPEED_STOP > sourceSpeed) {
 275             return 0.0;
 276         }
 277     }
 278     else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
 279         if (D_MIDDLE_SPEED_STOP > sourceSpeed) {
 280             return 0.0;
 281         }
 282     }
 283     else {
 284         if (D_HARD_SPEED_STOP > sourceSpeed) {
 285             return 0.0;
 286         }
 287     }
 288
 289     double brakeX;
 290     if (D_S_SPEED > sourceSpeed) {
 291         if (D_SOFT_BRAKE_MAX > m_brakeVal) {
 292             brakeX = D_S_SPEED_S;
 293         }
 294         else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
 295             brakeX = D_S_SPEED_M;
 296         }
 297         else {
 298             brakeX = D_S_SPEED_H;
 299         }
 300     }
 301     else if (D_M_SPEED > sourceSpeed) {
 302         if (D_SOFT_BRAKE_MAX > m_brakeVal) {
 303             brakeX = D_M_SPEED_S;
 304         }
 305         else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
 306             brakeX = D_M_SPEED_M;
 307         }
 308         else {
 309             brakeX = D_M_SPEED_H;
 310         }
 311     }
 312     else {
 313         if (D_SOFT_BRAKE_MAX > m_brakeVal) {
 314             brakeX = D_H_SPEED_S;
 315         }
 316         else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
 317             brakeX = D_H_SPEED_M;
 318         }
 319         else {
 320             brakeX = D_H_SPEED_H;
 321         }
 322     }
 323     return (sourceSpeed - (sourceSpeed * brakeX));
 324 }
 325
 326 /******************************************
 327  * Somewhat Gear
 328 ******************************************/
 329 /**
 330  * @brief CAvgGear
 331  *        Constructor
 332  */
 333 CAvgGear::CAvgGear()
 334 {
 335     m_transmission = E_SHIFT_PARKING;
 336     m_transmissionOLD = m_transmission;
 337     m_transmissionValue = D_SHIFT_VALUE_PARKING;
 338 }
 339
 340 /**
 341  * @brief ~CAvgGear
 342  *        Destructor
 343  */
 344 CAvgGear::~CAvgGear()
 345 {
 346
 347 }
 348
 349 /**
 350  * @brief getGearRatio
 351  * @param speed
 352  * @param bDown true:speed down
 353  * @return Gear Ratio
 354  */
 355 const double gr1 = 1.0 / 4.1;
 356 const double gr2 = 1.0 / 2.8;
 357 const double gr3 = 1.0 / 1.8;
 358 const double grD1 = 1.0 / 2.3;
 359 const double grD2 = 1.0 / 1.4;
 360 const double grD3 = 1.0 / 1.0;
 361 const double grD4 = 1.0 / 0.8;
 362 const double grD5 = 1.0 / 0.46;
 363 const double grR = 1.0 / 3.1;
 364
 365 const double atd1 = 9.0l;
 366 const double atd2 = 15.0l;
 367 const double atd3 = 23.0l;
 368 const double atd4 = 32.0l;
 369 const double atd5 = 42.0l;
 370
 371 double CAvgGear::getGearRatio(double speed, bool bDown) const
 372 {
 373     double gr;
 374     switch (m_transmission) {
 375     case E_SHIFT_FIRST:
 376         gr = gr1;
 377         break;
 378     case E_SHIFT_SECOND:
 379         gr = gr2;
 380         break;
 381     case E_SHIFT_THIRD:
 382         gr = gr3;
 383     case E_SHIFT_DRIVE:
 384         // Speed by changing the gear ratio
 385         if (atd1 > speed) {
 386             gr = grD1;
 387         }
 388         else if (atd2 > speed) {
 389             gr = grD2;
 390             if (true == bDown) {
 391                 gr = gr1;
 392             }
 393         }
 394         else if (atd3 > speed) {
 395             gr = grD3;
 396             if (true == bDown) {
 397                 gr = gr2;
 398             }
 399         }
 400         else if (atd4 > speed) {
 401             gr = grD4;
 402             if (true == bDown) {
 403                 gr = gr3;
 404             }
 405         }
 406         else {
 407             gr = grD5;
 408             if (true == bDown) {
 409                 gr = grD3;
 410             }
 411         }
 412         break;
 413     case E_SHIFT_REVERSE:
 414         gr = grR;
 415         break;
 416     case E_SHIFT_PARKING:
 417     case E_SHIFT_NEUTRAL:
 418         gr = 0.0l;
 419         break;
 420     case E_SHIFT_MT_FIRST:
 421         gr = gr1;
 422         break;
 423     case E_SHIFT_MT_SECOND:
 424         gr = grD1;
 425         break;
 426     case E_SHIFT_MT_THIRD:
 427         gr = grD2;
 428         break;
 429     case E_SHIFT_MT_FOURTH:
 430         gr = grD3;
 431         break;
 432     case E_SHIFT_MT_FIFTH:
 433         gr = grD4;
 434         break;
 435     case E_SHIFT_MT_SIXTH:
 436         gr = grD5;
 437         break;
 438     default:
 439         gr = 0.0l;
 440         break;
 441     }
 442     return gr;
 443 }
 444
 445 /**
 446  * @brief setShiftUp
 447  */
 448 void CAvgGear::setShiftDown()
 449 {
 450     switch (m_transmission) {
 451     case E_SHIFT_PARKING:
 452         m_transmissionOLD = m_transmission;
 453         m_transmission = E_SHIFT_REVERSE; // PARKING -> REVERSE
 454         m_transmissionValue = D_SHIFT_VALUE_REVERSE;
 455         break;
 456     case E_SHIFT_REVERSE:
 457         m_transmissionOLD = m_transmission;
 458         m_transmission = E_SHIFT_NEUTRAL; // REVERSE -> NEUTRAL
 459         m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
 460         break;
 461     case E_SHIFT_NEUTRAL:
 462         m_transmissionOLD = m_transmission;
 463         m_transmission = E_SHIFT_DRIVE;   // NEUTRAL -> DRIVE
 464         m_transmissionValue = D_SHIFT_VALUE_DRIVE;
 465         break;
 466     case E_SHIFT_DRIVE:
 467         m_transmissionOLD = m_transmission;
 468         m_transmission = E_SHIFT_THIRD;   // DRIVE   -> THIRD
 469         m_transmissionValue = D_SHIFT_VALUE_THIRD;
 470         break;
 471     case E_SHIFT_THIRD:
 472         m_transmissionOLD = m_transmission;
 473         m_transmission = E_SHIFT_SECOND;  // THIRD   -> SECOND
 474         m_transmissionValue = D_SHIFT_VALUE_SECOND;
 475         break;
 476     case E_SHIFT_SECOND:
 477         m_transmissionOLD = m_transmission;
 478         m_transmission = E_SHIFT_FIRST;   // SECOND  -> FIRST
 479         m_transmissionValue = D_SHIFT_VALUE_FIRST;
 480         break;
 481     case E_SHIFT_FIRST:
 482         m_transmissionOLD = m_transmission;
 483         m_transmission = E_SHIFT_FIRST;   // NO CHANGE
 484         m_transmissionValue = D_SHIFT_VALUE_FIRST;
 485         break;
 486     }
 487 }
 488
 489 /**
 490  * @brief setShiftUp
 491  */
 492 void CAvgGear::setShiftUp()
 493 {
 494     switch (m_transmission) {
 495     case E_SHIFT_FIRST:
 496         m_transmissionOLD = m_transmission;
 497         m_transmission = E_SHIFT_SECOND;  // FIRST   -> SECOND
 498         m_transmissionValue = D_SHIFT_VALUE_SECOND;
 499         break;
 500     case E_SHIFT_SECOND:
 501         m_transmissionOLD = m_transmission;
 502         m_transmission = E_SHIFT_THIRD;   // SECOND  -> THIRD
 503         m_transmissionValue = D_SHIFT_VALUE_THIRD;
 504         break;
 505     case E_SHIFT_THIRD:
 506         m_transmissionOLD = m_transmission;
 507         m_transmission = E_SHIFT_DRIVE;   // THIRD -> DRIVE
 508         m_transmissionValue = D_SHIFT_VALUE_DRIVE;
 509         break;
 510     case E_SHIFT_DRIVE:
 511         m_transmissionOLD = m_transmission;
 512         m_transmission = E_SHIFT_NEUTRAL; // DRIVE -> NEUTRAL
 513         m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
 514         break;
 515     case E_SHIFT_NEUTRAL:
 516         m_transmissionOLD = m_transmission;
 517         m_transmission = E_SHIFT_REVERSE; // NEUTRAL -> REVERSE
 518         m_transmissionValue = D_SHIFT_VALUE_REVERSE;
 519         break;
 520     case E_SHIFT_REVERSE:
 521         m_transmissionOLD = m_transmission;
 522         m_transmission = E_SHIFT_PARKING; // REVERSE  -> PARKING
 523         m_transmissionValue = D_SHIFT_VALUE_PARKING;
 524         break;
 525     case E_SHIFT_PARKING:
 526         m_transmissionOLD = m_transmission;
 527         m_transmission = E_SHIFT_PARKING; // NO CHANGE
 528         m_transmissionValue = D_SHIFT_VALUE_PARKING;
 529         break;
 530     }
 531 }
 532
 533 void CAvgGear::setShiftMT(E_AT_GEAR tm) {
 534     switch(tm) {
 535     case E_SHIFT_MT_FIRST :
 536         m_transmissionOLD = m_transmission;
 537         m_transmission = E_SHIFT_MT_FIRST;
 538         m_transmissionValue = D_SHIFT_VALUE_FIRST;
 539         break;
 540     case E_SHIFT_MT_SECOND :
 541         m_transmissionOLD = m_transmission;
 542         m_transmission = E_SHIFT_MT_SECOND;
 543         m_transmissionValue = D_SHIFT_VALUE_SECOND;
 544         break;
 545     case E_SHIFT_MT_THIRD :
 546         m_transmissionOLD = m_transmission;
 547         m_transmission = E_SHIFT_MT_THIRD;
 548         m_transmissionValue = D_SHIFT_VALUE_THIRD;
 549         break;
 550     case E_SHIFT_MT_FOURTH :
 551         m_transmissionOLD = m_transmission;
 552         m_transmission = E_SHIFT_MT_FOURTH;
 553         m_transmissionValue = D_SHIFT_VALUE_DRIVE;
 554         break;
 555     case E_SHIFT_MT_FIFTH :
 556         m_transmissionOLD = m_transmission;
 557         m_transmission = E_SHIFT_MT_FIFTH;
 558         m_transmissionValue = D_SHIFT_VALUE_FIFTH;
 559         break;
 560     case E_SHIFT_MT_SIXTH :
 561         m_transmissionOLD = m_transmission;
 562         m_transmission = E_SHIFT_MT_SIXTH;
 563         m_transmissionValue = D_SHIFT_VALUE_SIXTH;
 564         break;
 565     case E_SHIFT_REVERSE :
 566         m_transmissionOLD = m_transmission;
 567         m_transmission = E_SHIFT_REVERSE;
 568         m_transmissionValue = D_SHIFT_VALUE_REVERSE;
 569         break;
 570     case E_SHIFT_NEUTRAL :
 571         m_transmissionOLD = m_transmission;
 572         m_transmission = E_SHIFT_NEUTRAL;
 573         m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
 574         break;
 575     default :
 576         break;
 577     }
 578 }
 579
 580 /******************************************
 581  * Average Car
 582 ******************************************/
 583 /**
 584   * @brief CAvgCar
 585   *   constractor
 586   */
 587 CAvgCar::CAvgCar(const short RPMsz, const short SPDsz,
 588                              const short BRKsz)
 589     : m_brake(BRKsz), m_engine(RPMsz),
 590       m_speed(SPDsz? SPDsz: D_SAMPLE_SPACE_SPEED),
 591       m_accPedalOpen(D_SAMPLE_SPACE_ACCPEDAL)
 592 {
 593     m_odometer = 0.0;
 594     m_tp.tv_sec = 0;
 595     m_tp.tv_nsec = 0;
 596     m_bChgThrottle = false;
 597     m_valThrottle = -1;
 598     m_bChgBrake = false;
 599     m_valBrake = -1;
 600 }
 601
 602 /**
 603   * @brief CAvgCar
 604   *   destonstractor
 605   */
 606 CAvgCar::~CAvgCar()
 607 {
 608
 609 }
 610
 611 /**
 612  * @brief chgThrottle
 613  * @param throttle CAvgEngine parameter
 614  */
 615 void CAvgCar::chgThrottle(int throttle)
 616 {
 617     int x = abs((throttle - 32767));
 618     if (false == m_engine.isActive()) {
 619         m_engine.ignitionStart();
 620     }
 621     m_engine.chgThrottle(x);
 622     m_accPedalOpen.setSample(x);
 623     m_bChgThrottle = true;
 624     m_valThrottle  = x;
 625 }
 626
 627 /**
 628   * @brief chgBrake
 629   * @param BrakeVal CAvgBrake parameter
 630   */
 631 void CAvgCar::chgBrake(int brakeVal)
 632 {
 633     int x = abs((brakeVal - 32767));
 634     m_brake.chgBrake(x);
 635     m_bChgBrake = true;
 636     m_valBrake = x;
 637 }
 638
 639 /**
 640  * @brief calc
 641  *  speed and range calc
 642  */
 643 #define D_DOUBLE_SET_SPEED_MAX atd4
 644 void CAvgCar::calc()
 645 {
 646     double oldSpd = getSpeed();
 647     double rpm = getRPM();
 648     bool bDown = false;
 649     if (D_RPM_BOTTOM_BORDER > m_engine.getAvgRPM()){
 650         bDown = true;
 651     }
 652     double gearRatio = getGearRatio(oldSpd,bDown);
 653     double tmpX = rpm * gearRatio / 100;
 654
 655     double x = m_brake.getSpeed(tmpX);
 656     m_speed.setSample(x);
 657     /**
 658      * Processing for the acceleration
 659      */
 660     if ((oldSpd < x) && (atd4 > x)){
 661         m_speed.setSample(x);
 662         m_speed.setSample(x);
 663     }
 664     /**
 665      * Processing for the brake
 666      */
 667     if (true == m_brake.isOnBrake()) {
 668         if (oldSpd >= x) {
 669             if ((atd1 > oldSpd) && (0 == x)) {
 670                 m_speed.setSample(x);
 671                 m_speed.setSample(x);
 672                 m_speed.setSample(x);
 673                 m_speed.setSample(x);
 674             }
 675         }
 676     }
 677     double spd = m_speed.getAvg();
 678     struct timespec oldtp;
 679     oldtp.tv_sec = m_tp.tv_sec;
 680     oldtp.tv_nsec = m_tp.tv_nsec;
 681     clock_gettime(CLOCK_MONOTONIC, &m_tp);
 682     if (0.0 != spd) {
 683         long int sec  = m_tp.tv_sec  - oldtp.tv_sec; // sec
 684         long int nsec = m_tp.tv_nsec - oldtp.tv_nsec; // nano sec
 685         double r_nsec = (((double)sec * 1000000000l) +
 686                           (double)nsec) / 1000000000l; // get time(nanosec)
 687         m_currentRunMeter = (((double)(spd / 3600)) * 1000) * r_nsec; // get ran lengths
 688         if (true == isReverse()) {
 689             double x = 0 - m_currentRunMeter;
 690             m_currentRunMeter = x;
 691         }
 692         else if (true == isNeutral()) {
 693             if (true == isReverseLastTime()) {
 694                 double x = 0 - m_currentRunMeter;
 695                 m_currentRunMeter = x;
 696             }
 697         }
 698         m_tripmeter += m_currentRunMeter;
 699         m_odometer += m_currentRunMeter;
 700     }
 701     else {
 702         reset_old();
 703         m_currentRunMeter = 0.0l;
 704     }
 705 }
 706 /**
 707  * @brief updateAvg
 708  */
 709 void CAvgCar::updateAvg()
 710 {
 711     if ((false == m_bChgThrottle) && (-1 != m_valThrottle)) {
 712         m_engine.chgThrottle(m_valThrottle);
 713         m_accPedalOpen.setSample(m_valThrottle);
 714     }
 715     m_bChgThrottle = false;
 716     if ((false == m_bChgBrake) && (-1 != m_valBrake)){
 717         m_brake.chgBrake(m_valBrake);
 718     }
 719     m_bChgBrake = false;
 720     calc();
 721 }