Rev 5 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 5 | Rev 6 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <cstdint> |
1 | #include <cstdint> |
| - | 2 | #include <assert.h> |
|
| 2 | #include "timing.h" |
3 | #include "timing.h" |
| 3 | 4 | ||
| 4 | namespace |
5 | namespace |
| 5 | { |
6 | { |
| 6 | 7 | ||
| - | 8 | unsigned constexpr INTERP_SCALE = 256; |
|
| - | 9 | ||
| 7 | unsigned constexpr MAX_TIMING_POINTS = 8; |
10 | unsigned constexpr MAX_TIMING_POINTS = 8; |
| 8 | unsigned constexpr MAX_VACUUM_POINTS = 8; |
11 | unsigned constexpr MAX_VACUUM_POINTS = 8; |
| 9 | unsigned constexpr TimingScale = TIMING_SCALE; |
12 | unsigned constexpr TimingScale = TIMING_SCALE; |
| 10 | 13 | ||
| 11 | #pragma pack(push, 1) |
- | |
| 12 | typedef union |
- | |
| 13 | { |
- | |
| 14 | struct |
- | |
| 15 | { |
- | |
| 16 | int16_t val; |
14 | int16_t rpmMap[MAX_TIMING_POINTS] = {500, 750, 1000, 1500, 2500, 3500, 4500, 6000}; |
| 17 | int16_t result; |
15 | int16_t vacuumMap[MAX_VACUUM_POINTS] = {0, 166, 225, 300, 700, (int16_t)-1,(int16_t) -1, (int16_t)-1}; |
| 18 | }; |
- | |
| 19 | uint32_t u32; |
- | |
| 20 | } dataPoint; |
- | |
| 21 | #pragma pack(pop) |
- | |
| 22 | - | ||
| 23 | // points in RPM, degrees |
- | |
| 24 | dataPoint const timing_curve[MAX_TIMING_POINTS] = { |
16 | uint8_t mapping[MAX_VACUUM_POINTS][MAX_TIMING_POINTS] = { |
| 25 | {500, 3 * TimingScale}, |
17 | /* Table in degrees. */ |
| 26 | {750, 0 * TimingScale}, |
18 | /* row for 0mb = centrifugal only */ |
| 27 | {1000, 0 * TimingScale}, |
19 | {3, 0, 0, 12, 18, 22, 22, 15}, |
| 28 | {1500, 12 * TimingScale}, |
20 | /* row for 166 mB*/ |
| 29 | {2500, 18 * TimingScale}, |
21 | {3, 0, 0, 14, 20, 24, 24, 17}, |
| 30 | {3500, 22 * TimingScale}, |
22 | /* row for 225 mB */ |
| 31 | {4500, 22 * TimingScale}, |
23 | {3, 0, 0, 18, 24, 28, 28, 21}, |
| 32 | {6000, 15 * TimingScale}}; |
24 | /* row for 300 mB*/ |
| 33 | - | ||
| 34 | // points in mB of vacuum, degrees : initial points ordered |
25 | {3, 0, 0, 22, 28, 32, 32, 25}, |
| 35 | dataPoint const vacuum_curve[MAX_VACUUM_POINTS] = { |
- | |
| 36 | {0, 0 * TimingScale}, |
26 | /* row for 700 mB*/ |
| 37 | {166, 2 * TimingScale}, |
27 | {3, 0, 0, 22, 28, 32, 32, 25}, |
| 38 | {225, 6 * TimingScale}, |
28 | /* unused */ |
| 39 | {300, 10 * TimingScale}, |
29 | {0, 0, 0, 0, 0, 0, 0, 0}, |
| 40 | {700, 10 * TimingScale}, |
30 | /* unused */ |
| 41 | {700, 10 * TimingScale}, |
31 | {0, 0, 0, 0, 0, 0, 0, 0}, |
| 42 | {-1, -1}, // filler |
32 | /* unused */ |
| 43 | {-1, -1} // filler |
33 | {0, 0, 0, 0, 0, 0, 0, 0}}; |
| 44 | }; |
- | |
| 45 | 34 | ||
| 46 | // basic timing |
35 | // basic timing |
| 47 | const int baseTiming = 7 * TimingScale; |
36 | const int baseTiming = 7 * TimingScale; |
| - | 37 | }; |
|
| - | 38 | ||
| - | 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) |
|
| 48 | 46 | ||
| - | 47 | { |
|
| 49 | /// @brief Lookup a point using linear interpolation |
48 | // check lower bounds |
| 50 | /// @param point value to lookup |
49 | if (point < curve[0]) |
| - | 50 | { |
|
| - | 51 | *frac = 0; |
|
| - | 52 | return 0; |
|
| - | 53 | } |
|
| 51 | /// @param curve data point list |
54 | // check upper bounds |
| 52 | /// @param size number of data points in list |
55 | // find the upper boundary by looking for non -1 points |
| 53 | /// @return degrees * TIMING_SCALE |
56 | int upper = size - 1; |
| 54 | int lookup(int point, const dataPoint curve[], int size) |
57 | while (curve[upper] <= 0) |
| - | 58 | upper--; |
|
| 55 | 59 | ||
| - | 60 | if (point >= curve[upper]) |
|
| - | 61 | { |
|
| - | 62 | frac = 0; |
|
| - | 63 | return upper; |
|
| - | 64 | } |
|
| - | 65 | for (int pt = 1; pt <= upper; pt++) |
|
| 56 | { |
66 | { |
| 57 | // check lower bounds |
- | |
| 58 | if (point < curve[0].val) |
- | |
| 59 | return curve[0].result; |
- | |
| 60 | // check upper bounds |
- | |
| 61 | // find the upper boundary by looking for non -1 points |
- | |
| 62 | int upper = size - 1; |
- | |
| 63 | while (curve[upper].result <= 0) |
- | |
| 64 | upper--; |
- | |
| 65 | - | ||
| 66 | if (point >= curve[upper].val) |
67 | if ((point >= curve[pt - 1]) && (point < curve[pt])) |
| 67 | return curve[upper].result; |
- | |
| 68 | for (int pt = 1; pt <= upper; pt++) |
- | |
| 69 | { |
68 | { |
| 70 | if ((point >= curve[pt - 1].val) && (point < curve[pt].val)) |
- | |
| 71 | { |
- | |
| 72 | // how far along axis ? |
69 | // how far along axis ? |
| 73 | int offset = point - curve[pt - 1].val; |
70 | int offset = point - curve[pt - 1]; |
| 74 | 71 | ||
| 75 | int range1 = curve[pt].val - curve[pt - 1].val; |
72 | int range1 = curve[pt] - curve[pt - 1]; |
| 76 | 73 | ||
| 77 | int range2 = curve[pt].result - curve[pt - 1].result; |
74 | int range2 = INTERP_SCALE; |
| - | 75 | ||
| 78 | return ((offset * range2) / range1) + curve[pt - 1].result; |
76 | *frac = ((offset * range2) / range1); |
| 79 | } |
77 | return pt - 1; |
| 80 | } |
78 | } |
| 81 | return 0; // give up. |
- | |
| 82 | } |
79 | } |
| - | 80 | *frac = 0; |
|
| - | 81 | return -1; // give up. |
|
| 83 | }; |
82 | }; |
| 84 | 83 | ||
| 85 | extern "C" |
84 | extern "C" |
| 86 | { |
85 | { |
| 87 | 86 | ||
| 88 | int timing(int rpm, int vacuumMb) |
87 | int timing(int rpm, int vacuumMb) |
| 89 | { |
88 | { |
| - | 89 | int angle = 0; |
|
| - | 90 | /* lookup the interpolated RPM point */ |
|
| - | 91 | int16_t rpm_frac = 0; |
|
| 90 | int angle = lookup(rpm, timing_curve, sizeof(timing_curve) / sizeof(dataPoint)); |
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; |
|
| 91 | angle += lookup(vacuumMb, vacuum_curve, sizeof(vacuum_curve) / sizeof(dataPoint)); |
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 | ||
| 92 | angle += baseTiming; |
122 | angle += baseTiming; |
| - | 123 | ||
| - | 124 | ||
| - | 125 | assert((angle >= TimingScale * 7) && (angle < TimingScale * 50 )); |
|
| 93 | return angle; |
126 | return angle; |
| 94 | } |
127 | } |
| 95 | } |
128 | } |
| 96 | 129 | ||