2 * Copyright (c) 2013, TOYOTA MOTOR CORPORATION.
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
12 * RPM / Speed / Brake Value is determined by the average
14 * @date Apr-08-2013 create
22 /******************************************
24 ******************************************/
27 * @brief averageMachine
29 * @param sz average sample space size
31 averageMachine::averageMachine(const short sz)
36 memset(m_x, 0, sizeof(double) * sz);
44 * @brief ~averageMachine
47 averageMachine::~averageMachine()
54 * @param x sample value
56 void averageMachine::setSample(double x)
58 short next = m_posi + 1;
62 m_x[m_posi] = x; // store sample value
66 m_posi = next; // next position
67 m_avg = m_t / m_n; // get average
73 void averageMachine::reCalc()
76 for (int i = 0; i < m_sz; i++) {
82 /******************************************
83 * average Machine Integer
84 ******************************************/
87 * @brief iAverageMachine
90 iAverageMachine::iAverageMachine(const short sz)
95 memset(m_x, 0, sizeof(int) * sz);
104 * @brief ~iAverageMachine
107 iAverageMachine::~iAverageMachine()
114 * @param x sample value
116 void iAverageMachine::setSample(int x)
118 short next = m_posi + 1;
122 m_x[m_posi] = x; // store sample value
126 m_posi = next; // next position
127 m_avg = (double) m_t / (double) m_n; // get average
134 void iAverageMachine::reCalc()
137 for (int i = 0; i < m_sz; i++) {
140 m_avg = (double) m_t / (double) m_n; // get average
144 /******************************************
146 ******************************************/
152 CAvgEngine::CAvgEngine(const short sz)
153 :m_avgRPM(sz? sz: D_SAMPLE_SPACE_RPM)
162 CAvgEngine::~CAvgEngine()
169 void CAvgEngine::ignitionStart(int rpm)
173 m_rpm = (double) rpm;
179 * Gets the RPM from the throttle
180 * @param throttle 0 - 65535
182 void CAvgEngine::chgThrottle(int throttle)
184 double rpm = (((double) throttle) / 65534.0l) * 100.0l;
185 if (false == isActive()) {
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);
195 /******************************************
197 ******************************************/
201 * @param sz sample space size
203 CAvgBrake::CAvgBrake(const short sz)
204 :m_brk(sz? sz: D_SAMPLE_SPACE_BRAKE)
214 CAvgBrake::~CAvgBrake()
220 * Gets the Brake Infomation from the brake value
221 * @param brake 0 - 65535
223 void CAvgBrake::chgBrake(int brake)
225 m_brk.setSample(brake);
226 m_brakeVal = m_brk.getIAvg();
233 void CAvgBrake::judgeBrake()
236 if (D_LAG_BRAKE < m_brakeVal) {
243 * @param sourceSpeed speed
244 * @return Calculate the load on the brake
246 #define D_SOFT_SPEED_STOP 15.0l
247 #define D_MIDDLE_SPEED_STOP 22.0l
248 #define D_HARD_SPEED_STOP 30.0l
252 #define D_S_SPEED_S 0.4
253 #define D_S_SPEED_M 0.6
254 #define D_S_SPEED_H 0.8
258 #define D_M_SPEED_S 0.3
259 #define D_M_SPEED_M 0.5
260 #define D_M_SPEED_H 0.7
263 #define D_H_SPEED_S 0.1
264 #define D_H_SPEED_M 0.3
265 #define D_H_SPEED_H 0.5
267 double CAvgBrake::getSpeed(double sourceSpeed)
269 if (false == m_brake) {
273 if (D_SOFT_BRAKE_MAX > m_brakeVal) {
274 if (D_SOFT_SPEED_STOP > sourceSpeed) {
278 else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
279 if (D_MIDDLE_SPEED_STOP > sourceSpeed) {
284 if (D_HARD_SPEED_STOP > sourceSpeed) {
290 if (D_S_SPEED > sourceSpeed) {
291 if (D_SOFT_BRAKE_MAX > m_brakeVal) {
292 brakeX = D_S_SPEED_S;
294 else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
295 brakeX = D_S_SPEED_M;
298 brakeX = D_S_SPEED_H;
301 else if (D_M_SPEED > sourceSpeed) {
302 if (D_SOFT_BRAKE_MAX > m_brakeVal) {
303 brakeX = D_M_SPEED_S;
305 else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
306 brakeX = D_M_SPEED_M;
309 brakeX = D_M_SPEED_H;
313 if (D_SOFT_BRAKE_MAX > m_brakeVal) {
314 brakeX = D_H_SPEED_S;
316 else if (D_MIDDLE_BRAKE_MAX > m_brakeVal) {
317 brakeX = D_H_SPEED_M;
320 brakeX = D_H_SPEED_H;
323 return (sourceSpeed - (sourceSpeed * brakeX));
326 /******************************************
328 ******************************************/
335 m_transmission = E_SHIFT_PARKING;
336 m_transmissionOLD = m_transmission;
337 m_transmissionValue = D_SHIFT_VALUE_PARKING;
344 CAvgGear::~CAvgGear()
350 * @brief getGearRatio
352 * @param bDown true:speed down
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;
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;
371 double CAvgGear::getGearRatio(double speed, bool bDown) const
374 switch (m_transmission) {
384 // Speed by changing the gear ratio
388 else if (atd2 > speed) {
394 else if (atd3 > speed) {
400 else if (atd4 > speed) {
413 case E_SHIFT_REVERSE:
416 case E_SHIFT_PARKING:
417 case E_SHIFT_NEUTRAL:
420 case E_SHIFT_MT_FIRST:
423 case E_SHIFT_MT_SECOND:
426 case E_SHIFT_MT_THIRD:
429 case E_SHIFT_MT_FOURTH:
432 case E_SHIFT_MT_FIFTH:
435 case E_SHIFT_MT_SIXTH:
448 void CAvgGear::setShiftDown()
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;
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;
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;
467 m_transmissionOLD = m_transmission;
468 m_transmission = E_SHIFT_THIRD; // DRIVE -> THIRD
469 m_transmissionValue = D_SHIFT_VALUE_THIRD;
472 m_transmissionOLD = m_transmission;
473 m_transmission = E_SHIFT_SECOND; // THIRD -> SECOND
474 m_transmissionValue = D_SHIFT_VALUE_SECOND;
477 m_transmissionOLD = m_transmission;
478 m_transmission = E_SHIFT_FIRST; // SECOND -> FIRST
479 m_transmissionValue = D_SHIFT_VALUE_FIRST;
482 m_transmissionOLD = m_transmission;
483 m_transmission = E_SHIFT_FIRST; // NO CHANGE
484 m_transmissionValue = D_SHIFT_VALUE_FIRST;
492 void CAvgGear::setShiftUp()
494 switch (m_transmission) {
496 m_transmissionOLD = m_transmission;
497 m_transmission = E_SHIFT_SECOND; // FIRST -> SECOND
498 m_transmissionValue = D_SHIFT_VALUE_SECOND;
501 m_transmissionOLD = m_transmission;
502 m_transmission = E_SHIFT_THIRD; // SECOND -> THIRD
503 m_transmissionValue = D_SHIFT_VALUE_THIRD;
506 m_transmissionOLD = m_transmission;
507 m_transmission = E_SHIFT_DRIVE; // THIRD -> DRIVE
508 m_transmissionValue = D_SHIFT_VALUE_DRIVE;
511 m_transmissionOLD = m_transmission;
512 m_transmission = E_SHIFT_NEUTRAL; // DRIVE -> NEUTRAL
513 m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
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;
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;
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;
533 void CAvgGear::setShiftMT(E_AT_GEAR 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;
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;
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;
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;
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;
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;
565 case E_SHIFT_REVERSE :
566 m_transmissionOLD = m_transmission;
567 m_transmission = E_SHIFT_REVERSE;
568 m_transmissionValue = D_SHIFT_VALUE_REVERSE;
570 case E_SHIFT_NEUTRAL :
571 m_transmissionOLD = m_transmission;
572 m_transmission = E_SHIFT_NEUTRAL;
573 m_transmissionValue = D_SHIFT_VALUE_NEUTRAL;
580 /******************************************
582 ******************************************/
587 CAvgCar::CAvgCar(const short RPMsz, const short SPDsz,
589 : m_brake(BRKsz), m_engine(RPMsz),
590 m_speed(SPDsz? SPDsz: D_SAMPLE_SPACE_SPEED),
591 m_accPedalOpen(D_SAMPLE_SPACE_ACCPEDAL)
596 m_bChgThrottle = false;
613 * @param throttle CAvgEngine parameter
615 void CAvgCar::chgThrottle(int throttle)
617 int x = abs((throttle - 32767));
618 if (false == m_engine.isActive()) {
619 m_engine.ignitionStart();
621 m_engine.chgThrottle(x);
622 m_accPedalOpen.setSample(x);
623 m_bChgThrottle = true;
629 * @param BrakeVal CAvgBrake parameter
631 void CAvgCar::chgBrake(int brakeVal)
633 int x = abs((brakeVal - 32767));
641 * speed and range calc
643 #define D_DOUBLE_SET_SPEED_MAX atd4
646 double oldSpd = getSpeed();
647 double rpm = getRPM();
649 if (D_RPM_BOTTOM_BORDER > m_engine.getAvgRPM()){
652 double gearRatio = getGearRatio(oldSpd,bDown);
653 double tmpX = rpm * gearRatio / 100;
655 double x = m_brake.getSpeed(tmpX);
656 m_speed.setSample(x);
658 * Processing for the acceleration
660 if ((oldSpd < x) && (atd4 > x)){
661 m_speed.setSample(x);
662 m_speed.setSample(x);
665 * Processing for the brake
667 if (true == m_brake.isOnBrake()) {
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);
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);
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;
692 else if (true == isNeutral()) {
693 if (true == isReverseLastTime()) {
694 double x = 0 - m_currentRunMeter;
695 m_currentRunMeter = x;
698 m_tripmeter += m_currentRunMeter;
699 m_odometer += m_currentRunMeter;
703 m_currentRunMeter = 0.0l;
709 void CAvgCar::updateAvg()
711 if ((false == m_bChgThrottle) && (-1 != m_valThrottle)) {
712 m_engine.chgThrottle(m_valThrottle);
713 m_accPedalOpen.setSample(m_valThrottle);
715 m_bChgThrottle = false;
716 if ((false == m_bChgBrake) && (-1 != m_valBrake)){
717 m_brake.chgBrake(m_valBrake);