|
1 | 1 |
|
| 2 | +#include "calcRideParamsOnISR.task.h" |
| 3 | + |
| 4 | +#include "esp_log.h" |
2 | 5 | #include "freertos/FreeRTOS.h" |
3 | | -#include "freertos/task.h" |
4 | 6 | #include "freertos/queue.h" |
5 | | -#include "esp_log.h" |
6 | | - |
7 | | -#include "calcRideParamsOnISR.task.h" |
8 | | -#include "settings.h" |
| 7 | +#include "freertos/task.h" |
9 | 8 | #include "obc.h" |
| 9 | +#include "settings.h" |
10 | 10 |
|
11 | 11 | #define TAG "RIDE_CALC" |
12 | 12 |
|
13 | 13 | bool ignoreReed = false; |
14 | 14 |
|
15 | 15 | // handle server sychronization start event and set flags to block data |
16 | 16 | // during synchronization, calculations should be blocked |
17 | | -static void vOnSyncStartHandle(void* handler_args, esp_event_base_t base, int32_t id, void* event_data) { |
| 17 | +static void vOnSyncStartHandle(void *handler_args, esp_event_base_t base, int32_t id, void *event_data) { |
18 | 18 | ignoreReed = true; |
19 | 19 | } |
20 | 20 |
|
21 | 21 | // handle server sychronization start event and set flags to block data |
22 | 22 | // if synchronization finishes with success, finish the ride, by resseting rideParams, otherwise continue |
23 | | -static void vOnSyncFinishHandle(void* handler_args, esp_event_base_t base, int32_t id, void* event_data) { |
| 23 | +static void vOnSyncFinishHandle(void *handler_args, esp_event_base_t base, int32_t id, void *event_data) { |
| 24 | + bool reset = *((bool *)event_data); |
24 | 25 |
|
25 | | - bool reset = *((bool*) event_data); |
26 | | - |
27 | | - if (reset){ |
| 26 | + if (reset) { |
28 | 27 | rideParams.rotations = 0; |
29 | 28 | rideParams.totalRideTimeMs = 0; |
30 | 29 | rideParams.speed = 0.0; |
31 | 30 | rideParams.distance = 0.0; |
32 | 31 | rideParams.avgSpeed = 0.0; |
33 | 32 | rideParams.prevRotationTickCount = 0.0; |
34 | | - } |
| 33 | + } |
35 | 34 |
|
36 | 35 | ignoreReed = false; |
37 | 36 | } |
38 | 37 |
|
39 | | -void vCalcRideParamsOnISRTask(void* data) |
40 | | -{ |
| 38 | +void vCalcRideParamsOnISRTask(void *data) { |
41 | 39 | float currentSpeed; |
42 | 40 | uint16_t msBetweenRotationTicks; |
43 | 41 |
|
44 | 42 | ESP_LOGI(TAG, "Init reed switch"); |
45 | | - |
| 43 | + |
46 | 44 | ESP_ERROR_CHECK(esp_event_handler_register_with(obc_events_loop, OBC_EVENTS, SYNC_START_EVENT, vOnSyncStartHandle, NULL)); |
47 | 45 | ESP_ERROR_CHECK(esp_event_handler_register_with(obc_events_loop, OBC_EVENTS, SYNC_STOP_EVENT, vOnSyncFinishHandle, NULL)); |
48 | 46 |
|
49 | | - for(;;) { |
50 | | - if(xQueueReceive(reed_evt_queue, &rideParams.rotationTickCount, portMAX_DELAY) && !ignoreReed) { |
| 47 | + for (;;) { |
| 48 | + if (xQueueReceive(reed_evt_queue, &rideParams.rotationTickCount, portMAX_DELAY) && !ignoreReed) { |
51 | 49 | // TODO create buffer for reed time impulses before calculating time and speed |
52 | 50 | // TODO block rideParams while calculating? |
53 | 51 | if (rideParams.prevRotationTickCount != 0) { |
54 | 52 | if (!rideParams.moving) { |
55 | 53 | esp_event_post_to(obc_events_loop, OBC_EVENTS, RIDE_START_EVENT, NULL, 0, portMAX_DELAY); |
56 | 54 | } |
57 | | - msBetweenRotationTicks = ((int) rideParams.rotationTickCount - (int) rideParams.prevRotationTickCount) * (int) portTICK_RATE_MS; |
| 55 | + msBetweenRotationTicks = ((int)rideParams.rotationTickCount - (int)rideParams.prevRotationTickCount) * (int)portTICK_PERIOD_MS; |
58 | 56 | rideParams.totalRideTimeMs += msBetweenRotationTicks; |
59 | 57 | // TODO sometimes tick count doesn't update even with queue? prevent speed = inf for now |
60 | 58 | if (msBetweenRotationTicks > 0.00) { |
61 | | - currentSpeed = ( (float) CIRCUMFERENCE/1000000 ) / ( (float) msBetweenRotationTicks / 3600000 ); //km/h |
| 59 | + currentSpeed = ((float)CIRCUMFERENCE / 1000000) / ((float)msBetweenRotationTicks / 3600000); // km/h |
62 | 60 | // simple, soft filtering of noise (from cables?) - should't introduce any discrepancy to actual data |
63 | 61 | // and also could be used to detect unplanned, immediate stops |
64 | 62 | // max reasonable acceleration is 5.8 m/s/s |
65 | | - if (abs(currentSpeed - rideParams.speed) > 30) { |
| 63 | + if (abs((int)(currentSpeed - rideParams.speed)) > 30) { |
66 | 64 | continue; |
67 | 65 | } |
68 | 66 |
|
69 | 67 | rideParams.moving = true; |
70 | 68 | rideParams.rotations++; |
71 | 69 | rideParams.speed = currentSpeed; |
72 | | - rideParams.distance += (float)CIRCUMFERENCE/1000000; |
73 | | - rideParams.totalDistance += (float)CIRCUMFERENCE/1000000; |
74 | | - rideParams.avgSpeed = rideParams.distance / ( (float) rideParams.totalRideTimeMs / 3600000 ); |
| 70 | + rideParams.distance += (float)CIRCUMFERENCE / 1000000; |
| 71 | + rideParams.totalDistance += (float)CIRCUMFERENCE / 1000000; |
| 72 | + rideParams.avgSpeed = rideParams.distance / ((float)rideParams.totalRideTimeMs / 3600000); |
75 | 73 | if (rideParams.speed > rideParams.maxSpeed) { |
76 | 74 | rideParams.maxSpeed = rideParams.speed; |
77 | 75 | } |
78 | 76 | } |
79 | | - |
80 | 77 | } |
81 | | - rideParams.prevRotationTickCount = rideParams.rotationTickCount; |
| 78 | + rideParams.prevRotationTickCount = rideParams.rotationTickCount; |
82 | 79 |
|
83 | 80 | // ESP_LOGI(TAG, "[REED] count: %d, speed: %0.2f, diff: %d, distance: %0.2f", rideParams.rotations, rideParams.speed, msBetweenRotationTicks, rideParams.distance); |
84 | 81 | // ESP_LOGI(TAG, "[AVGS] speed: %0.2f", rideParams.avgSpeed); |
|
0 commit comments