Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 5 | mjames | 1 | #include <cstdint> |
| 6 | mjames | 2 | #include <assert.h> |
| 5 | mjames | 3 | #include "timing.h" |
| 4 | |||
| 5 | namespace |
||
| 6 | { |
||
| 7 | |||
| 6 | mjames | 8 | unsigned constexpr INTERP_SCALE = 256; |
| 9 | |||
| 5 | mjames | 10 | unsigned constexpr MAX_TIMING_POINTS = 8; |
| 11 | unsigned constexpr MAX_VACUUM_POINTS = 8; |
||
| 12 | unsigned constexpr TimingScale = TIMING_SCALE; |
||
| 13 | |||
| 7 | mjames | 14 | int16_t rpmMap[MAX_TIMING_POINTS] = {400, 750, 1000, 1500, 2500, 3500, 4500, 6000}; |
| 6 | mjames | 15 | int16_t vacuumMap[MAX_VACUUM_POINTS] = {0, 166, 225, 300, 700, (int16_t)-1,(int16_t) -1, (int16_t)-1}; |
| 16 | uint8_t mapping[MAX_VACUUM_POINTS][MAX_TIMING_POINTS] = { |
||
| 17 | /* Table in degrees. */ |
||
| 18 | /* row for 0mb = centrifugal only */ |
||
| 7 | mjames | 19 | {12, 7, 7, 19, 25, 29, 29, 22}, |
| 6 | mjames | 20 | /* row for 166 mB*/ |
| 7 | mjames | 21 | {12, 7, 7, 21, 27, 31, 31, 24}, |
| 6 | mjames | 22 | /* row for 225 mB */ |
| 7 | mjames | 23 | {12, 7, 7, 25, 31, 35, 35, 28}, |
| 6 | mjames | 24 | /* row for 300 mB*/ |
| 7 | mjames | 25 | {12, 7, 7, 29, 35, 39, 39, 33}, |
| 6 | mjames | 26 | /* row for 700 mB*/ |
| 7 | mjames | 27 | {12, 7, 7, 29, 35, 39, 39, 33}, |
| 6 | mjames | 28 | /* unused */ |
| 29 | {0, 0, 0, 0, 0, 0, 0, 0}, |
||
| 30 | /* unused */ |
||
| 31 | {0, 0, 0, 0, 0, 0, 0, 0}, |
||
| 32 | /* unused */ |
||
| 33 | {0, 0, 0, 0, 0, 0, 0, 0}}; |
||
| 5 | mjames | 34 | |
| 7 | mjames | 35 | |
| 6 | mjames | 36 | }; |
| 5 | mjames | 37 | |
| 6 | mjames | 38 | /// @brief Lookup a point using linear interpolation |
| 39 | /// @param point value to lookup |
||
| 40 | /// @param curve data point list |
||
| 41 | /// @param size number of data points in list |
||
| 42 | /// @param [out] frac fraction of distance between points |
||
| 43 | /// @return index of first point |
||
| 44 | int lookup(int point, int16_t const curve[], int size, int16_t *frac) |
||
| 5 | mjames | 45 | |
| 6 | mjames | 46 | { |
| 47 | // check lower bounds |
||
| 48 | if (point < curve[0]) |
||
| 5 | mjames | 49 | { |
| 6 | mjames | 50 | *frac = 0; |
| 51 | return 0; |
||
| 52 | } |
||
| 53 | // check upper bounds |
||
| 54 | // find the upper boundary by looking for non -1 points |
||
| 55 | int upper = size - 1; |
||
| 56 | while (curve[upper] <= 0) |
||
| 57 | upper--; |
||
| 5 | mjames | 58 | |
| 6 | mjames | 59 | if (point >= curve[upper]) |
| 60 | { |
||
| 61 | frac = 0; |
||
| 62 | return upper; |
||
| 63 | } |
||
| 64 | for (int pt = 1; pt <= upper; pt++) |
||
| 65 | { |
||
| 66 | if ((point >= curve[pt - 1]) && (point < curve[pt])) |
||
| 5 | mjames | 67 | { |
| 6 | mjames | 68 | // how far along axis ? |
| 69 | int offset = point - curve[pt - 1]; |
||
| 5 | mjames | 70 | |
| 6 | mjames | 71 | int range1 = curve[pt] - curve[pt - 1]; |
| 5 | mjames | 72 | |
| 6 | mjames | 73 | int range2 = INTERP_SCALE; |
| 74 | |||
| 75 | *frac = ((offset * range2) / range1); |
||
| 76 | return pt - 1; |
||
| 5 | mjames | 77 | } |
| 78 | } |
||
| 6 | mjames | 79 | *frac = 0; |
| 80 | return -1; // give up. |
||
| 5 | mjames | 81 | }; |
| 82 | |||
| 83 | extern "C" |
||
| 84 | { |
||
| 85 | |||
| 86 | int timing(int rpm, int vacuumMb) |
||
| 87 | { |
||
| 6 | mjames | 88 | int angle = 0; |
| 89 | /* lookup the interpolated RPM point */ |
||
| 90 | int16_t rpm_frac = 0; |
||
| 91 | int rpm_index = lookup(rpm, rpmMap, MAX_TIMING_POINTS, &rpm_frac); |
||
| 92 | |||
| 93 | /* lookup the interpolated vacuum point */ |
||
| 94 | int16_t vacuum_frac = 0; |
||
| 95 | int vacuum_index = lookup(vacuumMb, vacuumMap, MAX_VACUUM_POINTS, &vacuum_frac); |
||
| 96 | |||
| 97 | /* perform a bilinear mapping */ |
||
| 98 | int top_advance; |
||
| 99 | // we now have a position between two points in X and Y |
||
| 100 | if (rpm_frac == 0) |
||
| 101 | top_advance = mapping[vacuum_index][rpm_index] * INTERP_SCALE; |
||
| 102 | // if fractional part then interpolate points off the map |
||
| 103 | else |
||
| 104 | top_advance = mapping[vacuum_index][rpm_index] * (INTERP_SCALE - rpm_frac) + mapping[vacuum_index][rpm_index + 1] * rpm_frac; |
||
| 105 | |||
| 106 | int bottom_advance; |
||
| 107 | // if no fractional part, then the top and bottom advance point is the same |
||
| 108 | if (vacuum_frac == 0) |
||
| 109 | { |
||
| 110 | angle = top_advance * TimingScale / INTERP_SCALE ; |
||
| 111 | } |
||
| 112 | else |
||
| 113 | { |
||
| 114 | bottom_advance = mapping[vacuum_index + 1][rpm_index] * (INTERP_SCALE - rpm_frac) + mapping[vacuum_index + 1][rpm_index + 1] * rpm_frac; |
||
| 115 | /* interpolate down Y axis this time */ |
||
| 116 | int advance = top_advance * (INTERP_SCALE - vacuum_frac) + bottom_advance * vacuum_frac; |
||
| 117 | /* point is scaled by two multiplications */ |
||
| 118 | angle = advance * TimingScale / (INTERP_SCALE * INTERP_SCALE); |
||
| 119 | } |
||
| 120 | |||
| 121 | assert((angle >= TimingScale * 7) && (angle < TimingScale * 50 )); |
||
| 5 | mjames | 122 | return angle; |
| 123 | } |
||
| 124 | } |