task_pre.c 10.2 KB
Newer Older
Reto Da Forno's avatar
initial  
Reto Da Forno committed
1
/*
2
3
 * Copyright (c) 2020 - 2021, ETH Zurich, Computer Engineering Group (TEC)
 * All rights reserved.
Reto Da Forno's avatar
initial  
Reto Da Forno committed
4
 *
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
Reto Da Forno's avatar
initial  
Reto Da Forno committed
29
30
 */

31
32
33
34
/*
 * pre-communication task (reads messages from BOLT, handles timestamp requests)
 */

Reto Da Forno's avatar
initial  
Reto Da Forno committed
35
36
37
38
39
#include "main.h"


/* Global variables ----------------------------------------------------------*/

40
41
extern QueueHandle_t xQueueHandle_tx;
extern TIM_HandleTypeDef htim2;
Reto Da Forno's avatar
initial  
Reto Da Forno committed
42
43
44
45
46
47
48
49
50
51
52


/* Private define ------------------------------------------------------------*/

#ifndef PRE_TASK_RESUMED
#define PRE_TASK_RESUMED()
#define PRE_TASK_SUSPENDED()
#endif /* PRE_TASK_IND */


/* Private variables ---------------------------------------------------------*/
53
static uint64_t unix_timestamp_us     = 0;
54
55
static uint64_t bolt_trq_timestamp    = 0;
static uint64_t bolt_trq_hs_timestamp = 0;
56
static int32_t  average_drift_ppm     = 0;
57
58
static bool     timestamp_updated     = false;
static bool     timestamp_requested   = false;
Reto Da Forno's avatar
initial  
Reto Da Forno committed
59
60
61
62


/* Functions -----------------------------------------------------------------*/

63
64
65
66
67
68
69
static void init_time(void)
{
  /* load timestamp from RTC */
  unix_timestamp_us = rtc_get_unix_timestamp_ms() * 1000;
  LOG_INFO("UNIX timestamp %llu loaded from RTC", unix_timestamp_us);
}

Reto Da Forno's avatar
initial  
Reto Da Forno committed
70

71
void set_unix_timestamp_us(uint64_t ts_us)
72
{
73
  unix_timestamp_us = ts_us;
74
75
76
  timestamp_updated = true;
}

77
78

uint64_t get_unix_timestamp_us(void)
Reto Da Forno's avatar
initial  
Reto Da Forno committed
79
{
80
#if TIMESTAMP_USE_HS_TIMER
81
  return unix_timestamp_us + ((hs_timer_now() - bolt_trq_hs_timestamp) * (1000000LL - average_drift_ppm) / HS_TIMER_FREQUENCY);
82
#else /* TIMESTAMP_USE_HS_TIMER */
83
  return unix_timestamp_us + ((lptimer_now() - bolt_trq_timestamp) * (1000000LL - average_drift_ppm) / LPTIMER_SECOND);
84
#endif /* TIMESTAMP_USE_HS_TIMER */
Reto Da Forno's avatar
initial  
Reto Da Forno committed
85
86
}

87

Reto Da Forno's avatar
initial  
Reto Da Forno committed
88
89
void GPIO_PIN_3_Callback(void)
{
90
  bolt_trq_timestamp  = lptimer_now() - 20;   /* subtract wakeup + ISR + function call delays (estimate only) */
91
  timestamp_requested = true;
Reto Da Forno's avatar
initial  
Reto Da Forno committed
92
93
}

94
95

void handle_treq(void)
Reto Da Forno's avatar
initial  
Reto Da Forno committed
96
{
97
  /* timer 2 capture compare flag 4 set? (COM_TREQ) */
98
  if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_CC4)) {
99
100
101
102
    if (!lptimer_now_synced(&bolt_trq_timestamp, &bolt_trq_hs_timestamp)) {
      LOG_ERROR("failed to retrieve synchronized timestamps");
      return;
    }
103
104
105
    uint32_t curr_ticks       = (uint32_t)bolt_trq_hs_timestamp;
    uint32_t elapsed_hs_ticks = curr_ticks - htim2.Instance->CCR4;
    bolt_trq_hs_timestamp    -= elapsed_hs_ticks;
106
107
    bolt_trq_timestamp       -= HS_TIMER_TICKS_TO_LPTIMER(elapsed_hs_ticks);
    LOG_VERBOSE("timestamp request received %lums ago", HS_TIMER_TICKS_TO_MS(elapsed_hs_ticks));
108
109
110
111

    __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_CC4);      /* clear capture compare interrupt flag */
    __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_CC4OF);    /* clear capture overrun flag */

112
    timestamp_requested = true;
113
  }
Reto Da Forno's avatar
initial  
Reto Da Forno committed
114
115
}

116

117
void update_time(void)
Reto Da Forno's avatar
initial  
Reto Da Forno committed
118
{
119
120
  static uint64_t prev_trq_timestamp     = 0;
  static uint64_t prev_unix_timestamp_us = 0;
121
122
123
124

  /* only update the time if a timestamp request has been registered and a new timestamp has been received */
  if (timestamp_requested && timestamp_updated) {
    /* first, calculate the drift */
125
126
    if (prev_unix_timestamp_us) {
      int32_t  unix_ts_diff_us = (unix_timestamp_us - prev_unix_timestamp_us);
127
  #if TIMESTAMP_USE_HS_TIMER
128
      int32_t local_ts_diff_us = (uint64_t)HS_TIMER_TICKS_TO_US(bolt_trq_hs_timestamp - prev_trq_timestamp);
129
  #else /* TIMESTAMP_USE_HS_TIMER */
130
      int32_t local_ts_diff_us = (uint64_t)HS_TIMER_TICKS_TO_US(bolt_trq_timestamp - prev_trq_timestamp);
131
  #endif /* TIMESTAMP_USE_HS_TIMER */
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
      int32_t drift_ppm        = (int32_t)((int64_t)(local_ts_diff_us - unix_ts_diff_us) * 1000000LL / unix_ts_diff_us);
      if (drift_ppm < TIMESTAMP_MAX_DRIFT && drift_ppm > -TIMESTAMP_MAX_DRIFT) {
        if (drift_ppm > TIMESTAMP_TYPICAL_DRIFT || drift_ppm < -TIMESTAMP_TYPICAL_DRIFT) {
          LOG_WARNING("drift is larger than usual");
        }
        if (average_drift_ppm == 0) {
          average_drift_ppm = drift_ppm;
        } else {
          average_drift_ppm = (average_drift_ppm + drift_ppm) / 2;
        }
        /* note: a negative drift means the local time runs too slow */
        LOG_INFO("current drift compensation: %ldppm", average_drift_ppm);
        elwb_set_drift(average_drift_ppm);

      } else {
        LOG_WARNING("drift is too large (%ldppm)", drift_ppm);
        EVENT_WARNING(EVENT_SX1262_TSYNC_DRIFT, (uint32_t)drift_ppm);
149
      }
Reto Da Forno's avatar
initial  
Reto Da Forno committed
150
    }
151

152
    /* calculate the global time at the point where the next flood starts (note: use lptimer here in any case) */
153
    uint64_t new_time_us = unix_timestamp_us + (lptimer_get() - bolt_trq_timestamp) * 1000000 / LPTIMER_SECOND;
154
155
156
157
158
159
160
161

  #if TIMESTAMP_MAX_OFFSET_MS > 0
    /* calculate the difference between the actual time and the current network time */
    uint64_t curr_time_us = elwb_sched_get_time();
    int32_t delta = (int64_t)curr_time_us - (int64_t)new_time_us;
    if (delta > (TIMESTAMP_MAX_OFFSET_MS * 1000) || delta < -(TIMESTAMP_MAX_OFFSET_MS * 1000)) {
      elwb_sched_set_time(new_time_us);
      LOG_INFO("timestamp adjusted to %llu", new_time_us);
162
      EVENT_INFO(EVENT_SX1262_TIME_UPDATED, delta);
163
164
165
    } else {
      if (delta > 500) {
        /* slow down the clock speed to compensate the offset */
166
        elwb_set_drift(average_drift_ppm + 10);
167
168
      } else if (delta < -500) {
        /* speed up */
169
        elwb_set_drift(average_drift_ppm - 10);
170
171
172
173
174
      }
      LOG_INFO("current time offset: %ldus", delta);
    }
  #else
    /* adjust the network time */
175
    elwb_sched_set_time(new_time_us);
176
  #endif /* TIMESTAMP_MAX_OFFSET_MS */
177
178
179
180
181
182

  #if TIMESTAMP_USE_HS_TIMER
    prev_trq_timestamp   = bolt_trq_hs_timestamp;
  #else /* TIMESTAMP_USE_HS_TIMER */
    prev_trq_timestamp   = bolt_trq_timestamp;
  #endif /* TIMESTAMP_USE_HS_TIMER */
183
    prev_unix_timestamp_us = unix_timestamp_us;
Reto Da Forno's avatar
initial  
Reto Da Forno committed
184
  }
185
186
  timestamp_requested = false;
  timestamp_updated   = false;
Reto Da Forno's avatar
initial  
Reto Da Forno committed
187
188
}

189

Reto Da Forno's avatar
initial  
Reto Da Forno committed
190
191
192
/* pre communication round task */
void vTask_pre(void const * argument)
{
Reto Da Forno's avatar
Reto Da Forno committed
193
  LOG_VERBOSE("pre task started");
Reto Da Forno's avatar
initial  
Reto Da Forno committed
194
195
196
197
198
199

  /* check message size */
  if (sizeof(dpp_message_t) > DPP_MSG_PKT_LEN || DPP_MSG_PKT_LEN > BOLT_MAX_MSG_LEN) {
    FATAL_ERROR("invalid message size config");
  }

200
201
202
  init_time();

#if TIMESTAMP_USE_HS_TIMER
203
  /* configure input capture for TIM2_CH4 (PA3) */
204
  HAL_TIM_IC_Start(&htim2, TIM_CHANNEL_4);
205
#endif /* TIMESTAMP_USE_HS_TIMER */
Reto Da Forno's avatar
initial  
Reto Da Forno committed
206
207
208
209
210
211

  /* Infinite loop */
  for (;;)
  {
    /* wait for notification token (= explicitly granted permission to run) */
    PRE_TASK_SUSPENDED();
Reto Da Forno's avatar
Reto Da Forno committed
212
    xTaskNotifyWait(0, ULONG_MAX, NULL, portMAX_DELAY);
Reto Da Forno's avatar
initial  
Reto Da Forno committed
213
214
    PRE_TASK_RESUMED();

215
#if BOLT_ENABLE
Reto Da Forno's avatar
initial  
Reto Da Forno committed
216
    /* read from BOLT */
217
    static uint8_t  bolt_read_buffer[BOLT_MAX_MSG_LEN];
218
    uint32_t max_read_cnt = TRANSMIT_QUEUE_SIZE,
Reto Da Forno's avatar
initial  
Reto Da Forno committed
219
220
             forwarded = 0;
    /* only read as long as there is still space in the transmit queue */
Reto Da Forno's avatar
Reto Da Forno committed
221
    while (max_read_cnt && uxQueueSpacesAvailable(xQueueHandle_tx) && BOLT_DATA_AVAILABLE) {
Reto Da Forno's avatar
initial  
Reto Da Forno committed
222
223
      uint32_t len = bolt_read(bolt_read_buffer);
      if (!len) {
Reto Da Forno's avatar
Reto Da Forno committed
224
225
        /* not supposed to happen -> try to initialize BOLT */
        bolt_init();
226
        LOG_ERROR("bolt read failed");
Reto Da Forno's avatar
Reto Da Forno committed
227
        EVENT_ERROR(EVENT_SX1262_BOLT_ERROR, 0);
Reto Da Forno's avatar
initial  
Reto Da Forno committed
228
229
230
231
232
233
234
        break;
      }
      if (!process_message((dpp_message_t*)bolt_read_buffer, true)) {
        forwarded++;
      }
      max_read_cnt--;
    }
235
236
    if (max_read_cnt < TRANSMIT_QUEUE_SIZE) {
      LOG_INFO("%lu msg read from BOLT, %lu forwarded", TRANSMIT_QUEUE_SIZE - max_read_cnt, forwarded);
Reto Da Forno's avatar
initial  
Reto Da Forno committed
237
238
    }

239
    /* handle timestamp request (only if BOLT enabled) */
240
    handle_treq();
241
242
243
244
245
246
247
248
    if (!IS_HOST) {
      if (timestamp_requested) {
        send_timestamp(bolt_trq_timestamp);
        timestamp_requested = false;
      }
    } else {
      update_time();
    }
249

250
  #if BASEBOARD_TREQ_WATCHDOG && BASEBOARD
251
252
253
    static uint64_t last_treq = 0;      /* hs timestamp of last request */
    if (bolt_trq_hs_timestamp > last_treq) {
      last_treq = bolt_trq_hs_timestamp;
254
    }
255
    /* only use time request watchdog when baseboard is enabled */
256
    if (BASEBOARD_IS_ENABLED()) {
257
258
      bool powercycle = false;
      /* check when was the last time we got a time request */
259
260
      if (HS_TIMER_TICKS_TO_S(hs_timer_now() - last_treq) > BASEBOARD_TREQ_WATCHDOG) {
        last_treq  = hs_timer_now();
261
262
263
264
        powercycle = true;
      }
      if (powercycle) {
        /* power cycle the baseboard */
Reto Da Forno's avatar
Reto Da Forno committed
265
        LOG_WARNING("power-cycling baseboard (TREQ watchdog)");
266
        BASEBOARD_DISABLE();
267
        /* enable pin must be kept low for ~1s -> schedule pin release */
268
        if (!schedule_bb_command((elwb_get_time(0) / 1000000) + 2, CMD_SX1262_BASEBOARD_ENABLE, 0)) {
269
270
          /* we must wait and release the reset here */
          LOG_WARNING("failed to schedule baseboard enable");
271
272
          delay_us(1000000);
          BASEBOARD_ENABLE();
273
        }
Reto Da Forno's avatar
Reto Da Forno committed
274
      }
275
    } else {
276
      last_treq = hs_timer_now();
Reto Da Forno's avatar
Reto Da Forno committed
277
    }
278
279
  #endif /* BASEBOARD_TREQ_WATCHDOG */
#endif /* BOLT_ENABLE */
Reto Da Forno's avatar
Reto Da Forno committed
280

281
282
283
    /* wake the radio */
    radio_wakeup();

Reto Da Forno's avatar
Reto Da Forno committed
284
    //LOG_VERBOSE("pre task executed");
Reto Da Forno's avatar
initial  
Reto Da Forno committed
285
286
  }
}