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