git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@8301 35acf78f-673a-0410-8e92-d51de3d6d3f4

1 files changed, 702 insertions, 0 deletions

diff --git a/demos/STM32/RT-STM32F746G-DISCOVERY-LWIP-FATFS-USB/main.c b/demos/STM32/RT-STM32F746G-DISCOVERY-LWIP-FATFS-USB/main.c
new file mode 100644
index 000000000..093329ec8
--- /dev/null
+++ b/demos/STM32/RT-STM32F746G-DISCOVERY-LWIP-FATFS-USB/main.c
@@ -0,0 +1,702 @@
+/*

+    ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio

+

+    Licensed under the Apache License, Version 2.0 (the "License");

+    you may not use this file except in compliance with the License.

+    You may obtain a copy of the License at

+

+        http://www.apache.org/licenses/LICENSE-2.0

+

+    Unless required by applicable law or agreed to in writing, software

+    distributed under the License is distributed on an "AS IS" BASIS,

+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

+    See the License for the specific language governing permissions and

+    limitations under the License.

+*/

+

+#include <stdio.h>

+#include <string.h>

+

+#include "ch.h"

+#include "hal.h"

+#include "test.h"

+

+#include "chprintf.h"

+#include "shell.h"

+

+#include "lwipthread.h"

+#include "web/web.h"

+

+//#include "ff.h"

+

+#if 0

+/*===========================================================================*/

+/* Card insertion monitor.                                                   */

+/*===========================================================================*/

+

+#define POLLING_INTERVAL                10

+#define POLLING_DELAY                   10

+

+/**

+ * @brief   Card monitor timer.

+ */

+static virtual_timer_t tmr;

+

+/**

+ * @brief   Debounce counter.

+ */

+static unsigned cnt;

+

+/**

+ * @brief   Card event sources.

+ */

+static event_source_t inserted_event, removed_event;

+

+/**

+ * @brief   Insertion monitor timer callback function.

+ *

+ * @param[in] p         pointer to the @p BaseBlockDevice object

+ *

+ * @notapi

+ */

+static void tmrfunc(void *p) {

+  BaseBlockDevice *bbdp = p;

+

+  chSysLockFromISR();

+  if (cnt > 0) {

+    if (blkIsInserted(bbdp)) {

+      if (--cnt == 0) {

+        chEvtBroadcastI(&inserted_event);

+      }

+    }

+    else

+      cnt = POLLING_INTERVAL;

+  }

+  else {

+    if (!blkIsInserted(bbdp)) {

+      cnt = POLLING_INTERVAL;

+      chEvtBroadcastI(&removed_event);

+    }

+  }

+  chVTSetI(&tmr, MS2ST(POLLING_DELAY), tmrfunc, bbdp);

+  chSysUnlockFromISR();

+}

+

+/**

+ * @brief   Polling monitor start.

+ *

+ * @param[in] p         pointer to an object implementing @p BaseBlockDevice

+ *

+ * @notapi

+ */

+static void tmr_init(void *p) {

+

+  chEvtObjectInit(&inserted_event);

+  chEvtObjectInit(&removed_event);

+  chSysLock();

+  cnt = POLLING_INTERVAL;

+  chVTSetI(&tmr, MS2ST(POLLING_DELAY), tmrfunc, p);

+  chSysUnlock();

+}

+

+/*===========================================================================*/

+/* FatFs related.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief FS object.

+ */

+static FATFS SDC_FS;

+

+/* FS mounted and ready.*/

+static bool fs_ready = FALSE;

+

+/* Generic large buffer.*/

+static uint8_t fbuff[1024];

+

+static FRESULT scan_files(BaseSequentialStream *chp, char *path) {

+  FRESULT res;

+  FILINFO fno;

+  DIR dir;

+  int i;

+  char *fn;

+

+#if _USE_LFN

+  fno.lfname = 0;

+  fno.lfsize = 0;

+#endif

+  res = f_opendir(&dir, path);

+  if (res == FR_OK) {

+    i = strlen(path);

+    for (;;) {

+      res = f_readdir(&dir, &fno);

+      if (res != FR_OK || fno.fname[0] == 0)

+        break;

+      if (fno.fname[0] == '.')

+        continue;

+      fn = fno.fname;

+      if (fno.fattrib & AM_DIR) {

+        path[i++] = '/';

+        strcpy(&path[i], fn);

+        res = scan_files(chp, path);

+        if (res != FR_OK)

+          break;

+        path[--i] = 0;

+      }

+      else {

+        chprintf(chp, "%s/%s\r\n", path, fn);

+      }

+    }

+  }

+  return res;

+}

+#endif

+

+/*===========================================================================*/

+/* USB related stuff.                                                        */

+/*===========================================================================*/

+

+/*

+ * Endpoints to be used for USBD2.

+ */

+#define USBD2_DATA_REQUEST_EP           1

+#define USBD2_DATA_AVAILABLE_EP         1

+#define USBD2_INTERRUPT_REQUEST_EP      2

+

+/*

+ * Serial over USB Driver structure.

+ */

+static SerialUSBDriver SDU2;

+

+/*

+ * USB Device Descriptor.

+ */

+static const uint8_t vcom_device_descriptor_data[18] = {

+  USB_DESC_DEVICE       (0x0110,        /* bcdUSB (1.1).                    */

+                         0x02,          /* bDeviceClass (CDC).              */

+                         0x00,          /* bDeviceSubClass.                 */

+                         0x00,          /* bDeviceProtocol.                 */

+                         0x40,          /* bMaxPacketSize.                  */

+                         0x0483,        /* idVendor (ST).                   */

+                         0x5740,        /* idProduct.                       */

+                         0x0200,        /* bcdDevice.                       */

+                         1,             /* iManufacturer.                   */

+                         2,             /* iProduct.                        */

+                         3,             /* iSerialNumber.                   */

+                         1)             /* bNumConfigurations.              */

+};

+

+/*

+ * Device Descriptor wrapper.

+ */

+static const USBDescriptor vcom_device_descriptor = {

+  sizeof vcom_device_descriptor_data,

+  vcom_device_descriptor_data

+};

+

+/* Configuration Descriptor tree for a CDC.*/

+static const uint8_t vcom_configuration_descriptor_data[67] = {

+  /* Configuration Descriptor.*/

+  USB_DESC_CONFIGURATION(67,            /* wTotalLength.                    */

+                         0x02,          /* bNumInterfaces.                  */

+                         0x01,          /* bConfigurationValue.             */

+                         0,             /* iConfiguration.                  */

+                         0xC0,          /* bmAttributes (self powered).     */

+                         50),           /* bMaxPower (100mA).               */

+  /* Interface Descriptor.*/

+  USB_DESC_INTERFACE    (0x00,          /* bInterfaceNumber.                */

+                         0x00,          /* bAlternateSetting.               */

+                         0x01,          /* bNumEndpoints.                   */

+                         0x02,          /* bInterfaceClass (Communications

+                                           Interface Class, CDC section

+                                           4.2).                            */

+                         0x02,          /* bInterfaceSubClass (Abstract

+                                         Control Model, CDC section 4.3).   */

+                         0x01,          /* bInterfaceProtocol (AT commands,

+                                           CDC section 4.4).                */

+                         0),            /* iInterface.                      */

+  /* Header Functional Descriptor (CDC section 5.2.3).*/

+  USB_DESC_BYTE         (5),            /* bLength.                         */

+  USB_DESC_BYTE         (0x24),         /* bDescriptorType (CS_INTERFACE).  */

+  USB_DESC_BYTE         (0x00),         /* bDescriptorSubtype (Header

+                                           Functional Descriptor.           */

+  USB_DESC_BCD          (0x0110),       /* bcdCDC.                          */

+  /* Call Management Functional Descriptor. */

+  USB_DESC_BYTE         (5),            /* bFunctionLength.                 */

+  USB_DESC_BYTE         (0x24),         /* bDescriptorType (CS_INTERFACE).  */

+  USB_DESC_BYTE         (0x01),         /* bDescriptorSubtype (Call Management

+                                           Functional Descriptor).          */

+  USB_DESC_BYTE         (0x00),         /* bmCapabilities (D0+D1).          */

+  USB_DESC_BYTE         (0x01),         /* bDataInterface.                  */

+  /* ACM Functional Descriptor.*/

+  USB_DESC_BYTE         (4),            /* bFunctionLength.                 */

+  USB_DESC_BYTE         (0x24),         /* bDescriptorType (CS_INTERFACE).  */

+  USB_DESC_BYTE         (0x02),         /* bDescriptorSubtype (Abstract

+                                           Control Management Descriptor).  */

+  USB_DESC_BYTE         (0x02),         /* bmCapabilities.                  */

+  /* Union Functional Descriptor.*/

+  USB_DESC_BYTE         (5),            /* bFunctionLength.                 */

+  USB_DESC_BYTE         (0x24),         /* bDescriptorType (CS_INTERFACE).  */

+  USB_DESC_BYTE         (0x06),         /* bDescriptorSubtype (Union

+                                           Functional Descriptor).          */

+  USB_DESC_BYTE         (0x00),         /* bMasterInterface (Communication

+                                           Class Interface).                */

+  USB_DESC_BYTE         (0x01),         /* bSlaveInterface0 (Data Class

+                                           Interface).                      */

+  /* Endpoint 2 Descriptor.*/

+  USB_DESC_ENDPOINT     (USBD2_INTERRUPT_REQUEST_EP|0x80,

+                         0x03,          /* bmAttributes (Interrupt).        */

+                         0x0008,        /* wMaxPacketSize.                  */

+                         0xFF),         /* bInterval.                       */

+  /* Interface Descriptor.*/

+  USB_DESC_INTERFACE    (0x01,          /* bInterfaceNumber.                */

+                         0x00,          /* bAlternateSetting.               */

+                         0x02,          /* bNumEndpoints.                   */

+                         0x0A,          /* bInterfaceClass (Data Class

+                                           Interface, CDC section 4.5).     */

+                         0x00,          /* bInterfaceSubClass (CDC section

+                                           4.6).                            */

+                         0x00,          /* bInterfaceProtocol (CDC section

+                                           4.7).                            */

+                         0x00),         /* iInterface.                      */

+  /* Endpoint 3 Descriptor.*/

+  USB_DESC_ENDPOINT     (USBD2_DATA_AVAILABLE_EP,       /* bEndpointAddress.*/

+                         0x02,          /* bmAttributes (Bulk).             */

+                         0x0040,        /* wMaxPacketSize.                  */

+                         0x00),         /* bInterval.                       */

+  /* Endpoint 1 Descriptor.*/

+  USB_DESC_ENDPOINT     (USBD2_DATA_REQUEST_EP|0x80,    /* bEndpointAddress.*/

+                         0x02,          /* bmAttributes (Bulk).             */

+                         0x0040,        /* wMaxPacketSize.                  */

+                         0x00)          /* bInterval.                       */

+};

+

+/*

+ * Configuration Descriptor wrapper.

+ */

+static const USBDescriptor vcom_configuration_descriptor = {

+  sizeof vcom_configuration_descriptor_data,

+  vcom_configuration_descriptor_data

+};

+

+/*

+ * U.S. English language identifier.

+ */

+static const uint8_t vcom_string0[] = {

+  USB_DESC_BYTE(4),                     /* bLength.                         */

+  USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */

+  USB_DESC_WORD(0x0409)                 /* wLANGID (U.S. English).          */

+};

+

+/*

+ * Vendor string.

+ */

+static const uint8_t vcom_string1[] = {

+  USB_DESC_BYTE(38),                    /* bLength.                         */

+  USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */

+  'S', 0, 'T', 0, 'M', 0, 'i', 0, 'c', 0, 'r', 0, 'o', 0, 'e', 0,

+  'l', 0, 'e', 0, 'c', 0, 't', 0, 'r', 0, 'o', 0, 'n', 0, 'i', 0,

+  'c', 0, 's', 0

+};

+

+/*

+ * Device Description string.

+ */

+static const uint8_t vcom_string2[] = {

+  USB_DESC_BYTE(56),                    /* bLength.                         */

+  USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */

+  'C', 0, 'h', 0, 'i', 0, 'b', 0, 'i', 0, 'O', 0, 'S', 0, '/', 0,

+  'R', 0, 'T', 0, ' ', 0, 'V', 0, 'i', 0, 'r', 0, 't', 0, 'u', 0,

+  'a', 0, 'l', 0, ' ', 0, 'C', 0, 'O', 0, 'M', 0, ' ', 0, 'P', 0,

+  'o', 0, 'r', 0, 't', 0

+};

+

+/*

+ * Serial Number string.

+ */

+static const uint8_t vcom_string3[] = {

+  USB_DESC_BYTE(8),                     /* bLength.                         */

+  USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */

+  '0' + CH_KERNEL_MAJOR, 0,

+  '0' + CH_KERNEL_MINOR, 0,

+  '0' + CH_KERNEL_PATCH, 0

+};

+

+/*

+ * Strings wrappers array.

+ */

+static const USBDescriptor vcom_strings[] = {

+  {sizeof vcom_string0, vcom_string0},

+  {sizeof vcom_string1, vcom_string1},

+  {sizeof vcom_string2, vcom_string2},

+  {sizeof vcom_string3, vcom_string3}

+};

+

+/*

+ * Handles the GET_DESCRIPTOR callback. All required descriptors must be

+ * handled here.

+ */

+static const USBDescriptor *get_descriptor(USBDriver *usbp,

+                                           uint8_t dtype,

+                                           uint8_t dindex,

+                                           uint16_t lang) {

+

+  (void)usbp;

+  (void)lang;

+  switch (dtype) {

+  case USB_DESCRIPTOR_DEVICE:

+    return &vcom_device_descriptor;

+  case USB_DESCRIPTOR_CONFIGURATION:

+    return &vcom_configuration_descriptor;

+  case USB_DESCRIPTOR_STRING:

+    if (dindex < 4)

+      return &vcom_strings[dindex];

+  }

+  return NULL;

+}

+

+/**

+ * @brief   IN EP1 state.

+ */

+static USBInEndpointState ep1instate;

+

+/**

+ * @brief   OUT EP1 state.

+ */

+static USBOutEndpointState ep1outstate;

+

+/**

+ * @brief   EP1 initialization structure (both IN and OUT).

+ */

+static const USBEndpointConfig ep1config = {

+  USB_EP_MODE_TYPE_BULK,

+  NULL,

+  sduDataTransmitted,

+  sduDataReceived,

+  0x0040,

+  0x0040,

+  &ep1instate,

+  &ep1outstate,

+  2,

+  NULL

+};

+

+/**

+ * @brief   IN EP2 state.

+ */

+static USBInEndpointState ep2instate;

+

+/**

+ * @brief   EP2 initialization structure (IN only).

+ */

+static const USBEndpointConfig ep2config = {

+  USB_EP_MODE_TYPE_INTR,

+  NULL,

+  sduInterruptTransmitted,

+  NULL,

+  0x0010,

+  0x0000,

+  &ep2instate,

+  NULL,

+  1,

+  NULL

+};

+

+/*

+ * Handles the USB driver global events.

+ */

+static void usb_event(USBDriver *usbp, usbevent_t event) {

+

+  switch (event) {

+  case USB_EVENT_RESET:

+    return;

+  case USB_EVENT_ADDRESS:

+    return;

+  case USB_EVENT_CONFIGURED:

+    chSysLockFromISR();

+

+    /* Enables the endpoints specified into the configuration.

+       Note, this callback is invoked from an ISR so I-Class functions

+       must be used.*/

+    usbInitEndpointI(usbp, USBD2_DATA_REQUEST_EP, &ep1config);

+    usbInitEndpointI(usbp, USBD2_INTERRUPT_REQUEST_EP, &ep2config);

+

+    /* Resetting the state of the CDC subsystem.*/

+    sduConfigureHookI(&SDU2);

+

+    chSysUnlockFromISR();

+    return;

+  case USB_EVENT_SUSPEND:

+    chSysLockFromISR();

+

+    /* Disconnection event on suspend.*/

+    sduDisconnectI(&SDU2);

+

+    chSysUnlockFromISR();

+    return;

+  case USB_EVENT_WAKEUP:

+    return;

+  case USB_EVENT_STALLED:

+    return;

+  }

+  return;

+}

+

+/*

+ * USB driver configuration.

+ */

+static const USBConfig usbcfg = {

+  usb_event,

+  get_descriptor,

+  sduRequestsHook,

+  NULL

+};

+

+/*

+ * Serial over USB driver configuration.

+ */

+static const SerialUSBConfig serusbcfg = {

+  &USBD2,

+  USBD2_DATA_REQUEST_EP,

+  USBD2_DATA_AVAILABLE_EP,

+  USBD2_INTERRUPT_REQUEST_EP

+};

+

+/*===========================================================================*/

+/* Command line related.                                                     */

+/*===========================================================================*/

+

+#define SHELL_WA_SIZE   THD_WORKING_AREA_SIZE(2048)

+#define TEST_WA_SIZE    THD_WORKING_AREA_SIZE(256)

+

+static void cmd_mem(BaseSequentialStream *chp, int argc, char *argv[]) {

+  size_t n, size;

+

+  (void)argv;

+  if (argc > 0) {

+    chprintf(chp, "Usage: mem\r\n");

+    return;

+  }

+  n = chHeapStatus(NULL, &size);

+  chprintf(chp, "core free memory : %u bytes\r\n", chCoreGetStatusX());

+  chprintf(chp, "heap fragments   : %u\r\n", n);

+  chprintf(chp, "heap free total  : %u bytes\r\n", size);

+}

+

+static void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[]) {

+  static const char *states[] = {CH_STATE_NAMES};

+  thread_t *tp;

+

+  (void)argv;

+  if (argc > 0) {

+    chprintf(chp, "Usage: threads\r\n");

+    return;

+  }

+  chprintf(chp, "    addr    stack prio refs     state time\r\n");

+  tp = chRegFirstThread();

+  do {

+    chprintf(chp, "%08lx %08lx %4lu %4lu %9s\r\n",

+            (uint32_t)tp, (uint32_t)tp->p_ctx.r13,

+            (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),

+            states[tp->p_state]);

+    tp = chRegNextThread(tp);

+  } while (tp != NULL);

+}

+

+static void cmd_test(BaseSequentialStream *chp, int argc, char *argv[]) {

+  thread_t *tp;

+

+  (void)argv;

+  if (argc > 0) {

+    chprintf(chp, "Usage: test\r\n");

+    return;

+  }

+  tp = chThdCreateFromHeap(NULL, TEST_WA_SIZE, chThdGetPriorityX(),

+                           TestThread, chp);

+  if (tp == NULL) {

+    chprintf(chp, "out of memory\r\n");

+    return;

+  }

+  chThdWait(tp);

+}

+

+#if 0

+static void cmd_tree(BaseSequentialStream *chp, int argc, char *argv[]) {

+  FRESULT err;

+  uint32_t clusters;

+  FATFS *fsp;

+

+  (void)argv;

+  if (argc > 0) {

+    chprintf(chp, "Usage: tree\r\n");

+    return;

+  }

+  if (!fs_ready) {

+    chprintf(chp, "File System not mounted\r\n");

+    return;

+  }

+  err = f_getfree("/", &clusters, &fsp);

+  if (err != FR_OK) {

+    chprintf(chp, "FS: f_getfree() failed\r\n");

+    return;

+  }

+  chprintf(chp,

+           "FS: %lu free clusters, %lu sectors per cluster, %lu bytes free\r\n",

+           clusters, (uint32_t)SDC_FS.csize,

+           clusters * (uint32_t)SDC_FS.csize * (uint32_t)MMCSD_BLOCK_SIZE);

+  fbuff[0] = 0;

+  scan_files(chp, (char *)fbuff);

+}

+#endif

+

+static const ShellCommand commands[] = {

+  {"mem", cmd_mem},

+  {"threads", cmd_threads},

+  {"test", cmd_test},

+//  {"tree", cmd_tree},

+  {NULL, NULL}

+};

+

+static const ShellConfig shell_cfg1 = {

+  (BaseSequentialStream *)&SDU2,

+  commands

+};

+

+/*===========================================================================*/

+/* Main and generic code.                                                    */

+/*===========================================================================*/

+

+#if 0

+/*

+ * Card insertion event.

+ */

+static void InsertHandler(eventid_t id) {

+  FRESULT err;

+

+  (void)id;

+  /*

+   * On insertion SDC initialization and FS mount.

+   */

+  if (sdcConnect(&SDCD1))

+    return;

+

+  err = f_mount(&SDC_FS, "/", 1);

+  if (err != FR_OK) {

+    sdcDisconnect(&SDCD1);

+    return;

+  }

+  fs_ready = TRUE;

+}

+

+/*

+ * Card removal event.

+ */

+static void RemoveHandler(eventid_t id) {

+

+  (void)id;

+  sdcDisconnect(&SDCD1);

+  fs_ready = FALSE;

+}

+#endif

+

+/*

+ * Green LED blinker thread, times are in milliseconds.

+ */

+static THD_WORKING_AREA(waThread1, 128);

+static THD_FUNCTION(Thread1, arg) {

+

+  (void)arg;

+  chRegSetThreadName("blinker");

+  while (true) {

+//    palTogglePad(GPIOC, GPIOC_LED);

+//    chThdSleepMilliseconds(fs_ready ? 125 : 500);

+    chThdSleepMilliseconds(500);

+  }

+}

+

+/*

+ * Application entry point.

+ */

+int main(void) {

+  static thread_t *shelltp = NULL;

+//  static const evhandler_t evhndl[] = {

+//    InsertHandler,

+//    RemoveHandler

+//  };

+//  event_listener_t el0, el1;

+

+  /*

+   * System initializations.

+   * - HAL initialization, this also initializes the configured device drivers

+   *   and performs the board-specific initializations.

+   * - Kernel initialization, the main() function becomes a thread and the

+   *   RTOS is active.

+   * - lwIP subsystem initialization using the default configuration.

+   */

+  halInit();

+  chSysInit();

+  lwipInit(NULL);

+

+  /*

+   * Initializes a serial-over-USB CDC driver.

+   */

+  sduObjectInit(&SDU2);

+  sduStart(&SDU2, &serusbcfg);

+

+  /*

+   * Activates the USB driver and then the USB bus pull-up on D+.

+   * Note, a delay is inserted in order to not have to disconnect the cable

+   * after a reset.

+   */

+  usbDisconnectBus(serusbcfg.usbp);

+  chThdSleepMilliseconds(1500);

+  usbStart(serusbcfg.usbp, &usbcfg);

+  usbConnectBus(serusbcfg.usbp);

+

+  /*

+   * Shell manager initialization.

+   */

+  shellInit();

+

+  /*

+   * Activates the serial driver 1 and SDC driver 1 using default

+   * configuration.

+   */

+  sdStart(&SD1, NULL);

+//  sdcStart(&SDCD1, NULL);

+

+  /*

+   * Activates the card insertion monitor.

+   */

+//  tmr_init(&SDCD1);

+

+  /*

+   * Creates the blinker thread.

+   */

+  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

+

+  /*

+   * Creates the HTTP thread (it changes priority internally).

+   */

+  chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1,

+                    http_server, NULL);

+

+  /*

+   * Normal main() thread activity, in this demo it does nothing except

+   * sleeping in a loop and listen for events.

+   */

+//  chEvtRegister(&inserted_event, &el0, 0);

+//  chEvtRegister(&removed_event, &el1, 1);

+  while (true) {

+    if (!shelltp && (SDU2.config->usbp->state == USB_ACTIVE))

+      shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);

+    else if (chThdTerminatedX(shelltp)) {

+      chThdRelease(shelltp);    /* Recovers memory of the previous shell.   */

+      shelltp = NULL;           /* Triggers spawning of a new shell.        */

+    }

+    if (palReadPad(GPIOI, GPIOI_BUTTON_USER) != 0) {

+    }

+//    chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));

+    chThdSleepMilliseconds(500);

+  }

+}