From da0dac74bd8ed5cff55b665da1cbdb989ef52eb4 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 15 Jul 2015 13:59:24 +0200
Subject: [PATCH] [rotorcraft][linux] limit main/event loop to 1kHz

This is a kludge until we can better leverage threads and have real events.
Without this limit the event flags will constantly polled as fast as possible,
resulting on 100% cpu load on boards with an (RT)OS.
On bare metal boards this is not an issue, as you have nothing else running anyway.

Enable this for the bebop and ardrone2 by default.

Alternative to #1239 and should fix #1233
---
 conf/boards/ardrone2.makefile           |  3 +++
 conf/boards/bebop.makefile              |  3 +++
 sw/airborne/firmwares/rotorcraft/main.c | 24 ++++++++++++++++++++++++
 3 files changed, 30 insertions(+)

diff --git a/conf/boards/ardrone2.makefile b/conf/boards/ardrone2.makefile
index 7c57158a2e..41984f3f46 100644
--- a/conf/boards/ardrone2.makefile
+++ b/conf/boards/ardrone2.makefile
@@ -42,6 +42,9 @@ $(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL -D_GNU_SOURCE
 $(TARGET).CFLAGS += -DLINUX_LINK_STATIC
 $(TARGET).LDFLAGS += -static
 
+# limit main loop to 1kHz so ap doesn't need 100% cpu
+$(TARGET).CFLAGS += -DLIMIT_EVENT_POLLING
+
 # -----------------------------------------------------------------------
 
 # default LED configuration
diff --git a/conf/boards/bebop.makefile b/conf/boards/bebop.makefile
index 6d9b714bd8..89ee316203 100644
--- a/conf/boards/bebop.makefile
+++ b/conf/boards/bebop.makefile
@@ -38,6 +38,9 @@ $(TARGET).srcs +=  $(SRC_BOARD)/video.c
 $(TARGET).CFLAGS += -DLINUX_LINK_STATIC
 $(TARGET).LDFLAGS += -static
 
+# limit main loop to 1kHz so ap doesn't need 100% cpu
+$(TARGET).CFLAGS += -DLIMIT_EVENT_POLLING
+
 # -----------------------------------------------------------------------
 
 # default LED configuration
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 41a8f62213..b15fcc56a5 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -120,10 +120,34 @@ int main(void)
 {
   main_init();
 
+#if LIMIT_EVENT_POLLING
+  /* Limit main loop frequency to 1kHz.
+   * This is a kludge until we can better leverage threads and have real events.
+   * Without this limit the event flags will constantly polled as fast as possible,
+   * resulting on 100% cpu load on boards with an (RT)OS.
+   * On bare metal boards this is not an issue, as you have nothing else running anyway.
+   */
+  uint32_t t_begin = 0;
+  uint32_t t_diff = 0;
+  while (1) {
+    t_begin = get_sys_time_usec();
+
+    handle_periodic_tasks();
+    main_event();
+
+    /* sleep remaining time to limit to 1kHz */
+    t_diff = get_sys_time_usec() - t_begin;
+    if (t_diff < 1000) {
+      sys_time_usleep(1000 - t_diff);
+    }
+  }
+#else
   while (1) {
     handle_periodic_tasks();
     main_event();
   }
+#endif
+
   return 0;
 }
 #endif /* SITL */