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_transmissionValue = D_SHIFT_VALUE_PARKING;
 337 }
 338
 339 /**
 340  * @brief ~CAvgGear
 341  *        Destructor
 342  */
 343 CAvgGear::~CAvgGear()
 344 {
 345
 346 }
 347
 348 /**
 349  * @brief getGearRatio
 350  * @param speed
 351  * @param bDown true:speed down
 352  * @return Gear Ratio
 353  */
 354 const double gr1 = 1.0 / 4.1;
 355 const double gr2 = 1.0 / 2.8;
 356 const double gr3 = 1.0 / 1.8;
 357 const double grD1 = 1.0 / 2.3;
 358 const double grD2 = 1.0 / 1.4;
 359 const double grD3 = 1.0 / 1.0;
 360 const double grD4 = 1.0 / 0.8;
 361 const double grD5 = 1.0 / 0.46;
 362 const double grR = 1.0 / 3.1;
 363
 364 const double atd1 = 9.0l;
 365 const double atd2 = 15.0l;
 366 const double atd3 = 23.0l;
 367 const double atd4 = 32.0l;
 368 const double atd5 = 42.0l;
 369
 370 double CAvgGear::getGearRatio(double speed, bool bDown) const
 371 {
 372     double gr;
 373     switch (m_transmission) {
 374     case E_SHIFT_FIRST:
 375         gr = gr1;
 376         break;
 377     case E_SHIFT_SECOND:
 378         gr = gr2;
 379         break;
 380     case E_SHIFT_THIRD:
 381         gr = gr3;
 382     case E_SHIFT_DRIVE:
 383         // Speed by changing the gear ratio
 384         if (atd1 > speed) {
 385             gr = grD1;
 386         }
 387         else if (atd2 > speed) {
 388             gr = grD2;
 389             if (true == bDown) {
 390                 gr = gr1;
 391             }
 392         }
 393         else if (atd3 > speed) {
 394             gr = grD3;
 395             if (true == bDown) {
 396                 gr = gr2;
 397             }
 398         }
 399         else if (atd4 > speed) {
 400             gr = grD4;
 401             if (true == bDown) {
 402                 gr = gr3;
 403             }
 404         }
 405         else {
 406             gr = grD5;
 407             if (true == bDown) {
 408                 gr = grD3;
 409             }
 410         }
 411         break;
 412     case E_SHIFT_REVERSE:
 413         gr = grR;
 414         break;
 415     case E_SHIFT_PARKING:
 416     case E_SHIFT_NEUTRAL:
 417         gr = 0.0l;
 418         break;
 419     default:
 420         gr = 0.0l;
 421         break;
 422     }
 423     return gr;
 424 }
 425
 426 /**
 427  * @brief setShiftUp
 428  */
 429 void CAvgGear::setShiftDown()
 430 {
 431     switch (m_transmission) {
 432     case E_SHIFT_PARKING:
 433         m_transmission = E_SHIFT_REVERSE; // PARKING -> REVERSE
 434         m_transmissionValue = D_SHIFT_VALUE_REVERSE;
 435         break;
 436     case E_SHIFT_REVERSE:
 437         m_transmission = E_SHIFT_NEUTRAL; // REVERSE -> NEUTRAL
 438         m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
 439         break;
 440     case E_SHIFT_NEUTRAL:
 441         m_transmission = E_SHIFT_DRIVE;   // NEUTRAL -> DRIVE
 442         m_transmissionValue = D_SHIFT_VALUE_DRIVE;
 443         break;
 444     case E_SHIFT_DRIVE:
 445         m_transmission = E_SHIFT_THIRD;   // DRIVE   -> THIRD
 446         m_transmissionValue = D_SHIFT_VALUE_THIRD;
 447         break;
 448     case E_SHIFT_THIRD:
 449         m_transmission = E_SHIFT_SECOND;  // THIRD   -> SECOND
 450         m_transmissionValue = D_SHIFT_VALUE_SECOND;
 451         break;
 452     case E_SHIFT_SECOND:
 453         m_transmission = E_SHIFT_FIRST;   // SECOND  -> FIRST
 454         m_transmissionValue = D_SHIFT_VALUE_FIRST;
 455         break;
 456     case E_SHIFT_FIRST:
 457         m_transmission = E_SHIFT_FIRST;   // NO CHANGE
 458         m_transmissionValue = D_SHIFT_VALUE_FIRST;
 459         break;
 460     }
 461 }
 462
 463 /**
 464  * @brief setShiftUp
 465  */
 466 void CAvgGear::setShiftUp()
 467 {
 468     switch (m_transmission) {
 469     case E_SHIFT_FIRST:
 470         m_transmission = E_SHIFT_SECOND;  // FIRST   -> SECOND
 471         m_transmissionValue = D_SHIFT_VALUE_SECOND;
 472         break;
 473     case E_SHIFT_SECOND:
 474         m_transmission = E_SHIFT_THIRD;   // SECOND  -> THIRD
 475         m_transmissionValue = D_SHIFT_VALUE_THIRD;
 476         break;
 477     case E_SHIFT_THIRD:
 478         m_transmission = E_SHIFT_DRIVE;   // THIRD -> DRIVE
 479         m_transmissionValue = D_SHIFT_VALUE_DRIVE;
 480         break;
 481     case E_SHIFT_DRIVE:
 482         m_transmission = E_SHIFT_NEUTRAL; // DRIVE -> NEUTRAL
 483         m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
 484         break;
 485     case E_SHIFT_NEUTRAL:
 486         m_transmission = E_SHIFT_REVERSE; // NEUTRAL -> REVERSE
 487         m_transmissionValue = D_SHIFT_VALUE_REVERSE;
 488         break;
 489     case E_SHIFT_REVERSE:
 490         m_transmission = E_SHIFT_PARKING; // REVERSE  -> PARKING
 491         m_transmissionValue = D_SHIFT_VALUE_PARKING;
 492         break;
 493     case E_SHIFT_PARKING:
 494         m_transmission = E_SHIFT_PARKING; // NO CHANGE
 495         m_transmissionValue = D_SHIFT_VALUE_PARKING;
 496         break;
 497     }
 498 }
 499
 500 /******************************************
 501  * Average Car
 502 ******************************************/
 503 /**
 504   * @brief CAvgCar
 505   *   constractor
 506   */
 507 CAvgCar::CAvgCar(const short RPMsz, const short SPDsz,
 508                              const short BRKsz)
 509     : m_brake(BRKsz), m_engine(RPMsz),
 510       m_speed(SPDsz? SPDsz: D_SAMPLE_SPACE_SPEED),
 511       m_accPedalOpen(D_SAMPLE_SPACE_ACCPEDAL)
 512 {
 513     m_odometer = 0.0;
 514     m_tp.tv_sec = 0;
 515     m_tp.tv_nsec = 0;
 516     m_bChgThrottle = false;
 517     m_valThrottle = -1;
 518     m_bChgBrake = false;
 519     m_valBrake = -1;
 520 }
 521
 522 /**
 523   * @brief CAvgCar
 524   *   destonstractor
 525   */
 526 CAvgCar::~CAvgCar()
 527 {
 528
 529 }
 530
 531 /**
 532  * @brief chgThrottle
 533  * @param throttle CAvgEngine parameter
 534  */
 535 void CAvgCar::chgThrottle(int throttle)
 536 {
 537     int x = abs((throttle - 32767));
 538     if (false == m_engine.isActive()) {
 539         if ((true == isNeutral()) || (true == isParking())) {
 540             m_engine.ignitionStart(D_RPM_IGNITION_VALUE);
 541         }
 542
 543         for (int i; i < D_SAMPLE_SPACE_ACCPEDAL; i++) {
 544             m_accPedalOpen.setSample(x);
 545         }
 546
 547         return;
 548     }
 549     else {
 550         m_engine.chgThrottle(x);
 551         m_accPedalOpen.setSample(x);
 552         m_bChgThrottle = true;
 553         m_valThrottle  = x;
 554     }
 555 }
 556
 557 /**
 558   * @brief chgBrake
 559   * @param BrakeVal CAvgBrake parameter
 560   */
 561 void CAvgCar::chgBrake(int brakeVal)
 562 {
 563     int x = abs((brakeVal - 32767));
 564     m_brake.chgBrake(x);
 565     m_bChgBrake = true;
 566     m_valBrake = x;
 567 }
 568
 569 /**
 570  * @brief calc
 571  *  speed and range calc
 572  */
 573 #define D_DOUBLE_SET_SPEED_MAX atd4
 574 void CAvgCar::calc()
 575 {
 576     double oldSpd = getSpeed();
 577     double rpm = getRPM();
 578     bool bDown = false;
 579     if (D_RPM_BOTTOM_BORDER > m_engine.getAvgRPM()){
 580         bDown = true;
 581     }
 582     double gearRatio = getGearRatio(oldSpd,bDown);
 583     double tmpX = rpm * gearRatio / 100;
 584
 585     double x = m_brake.getSpeed(tmpX);
 586     m_speed.setSample(x);
 587     /**
 588      * Processing for the acceleration
 589      */
 590     if ((oldSpd < x) && (atd4 > x)){
 591         m_speed.setSample(x);
 592         m_speed.setSample(x);
 593     }
 594     /**
 595      * Processing for the brake
 596      */
 597     if (true == m_brake.isOnBrake()) {
 598         if (oldSpd >= x) {
 599             if ((atd1 > oldSpd) && (0 == x)) {
 600                 m_speed.setSample(x);
 601                 m_speed.setSample(x);
 602                 m_speed.setSample(x);
 603                 m_speed.setSample(x);
 604             }
 605         }
 606     }
 607     double spd = m_speed.getAvg();
 608     struct timespec oldtp;
 609     oldtp.tv_sec = m_tp.tv_sec;
 610     oldtp.tv_nsec = m_tp.tv_nsec;
 611     clock_gettime(CLOCK_MONOTONIC, &m_tp);
 612     if (0.0 != spd) {
 613         long int sec  = m_tp.tv_sec  - oldtp.tv_sec; // sec
 614         long int nsec = m_tp.tv_nsec - oldtp.tv_nsec; // nano sec
 615         double r_nsec = (((double)sec * 1000000000l) +
 616                           (double)nsec) / 1000000000l; // get time(nanosec)
 617         m_currentRunMeter = (((double)(spd / 3600)) * 1000) * r_nsec; // get ran lengths
 618         if (true == isReverse()) {
 619             double x = 0 - m_currentRunMeter;
 620             m_currentRunMeter = x;
 621         }
 622         m_tripmeter += m_currentRunMeter;
 623         m_odometer += m_currentRunMeter;
 624     }
 625     else {
 626         m_currentRunMeter = 0.0l;
 627     }
 628 }
 629 /**
 630  * @brief updateAvg
 631  */
 632 void CAvgCar::updateAvg()
 633 {
 634     if ((false == m_bChgThrottle) && (-1 != m_valThrottle)) {
 635         m_engine.chgThrottle(m_valThrottle);
 636         m_accPedalOpen.setSample(m_valThrottle);
 637     }
 638     m_bChgThrottle = false;
 639     if ((false == m_bChgBrake) && (-1 != m_valBrake)){
 640         m_brake.chgBrake(m_valBrake);
 641     }
 642     m_bChgBrake = false;
 643     calc();
 644 }