Add graceful shutdown support

Currently to shut-down the Crazyflie, the nRF51 simply cut the power
to thae system. This was working fine until we started implementing
SD-card functionality: when shutting down the system that way we have
huge chances of corrupting the SD-card filesystem. This has lead to
implementation of very inefficient sd-car logging procedure to decrease
the probability of corruption.

A better solution for the shutdown problem would be to implement a
graceful shutdown in the system ...

  ... when the nRF51 wants to shutdown the system (ie. after a button
      press), it sends a message to the stm32 requesting shutdown

  ... in the stm32, modules that wants to be notified of shutdown are
      called in sequence and return when they are ready for shutdown

  ... when all the module have returned, the stm32 sends to the nRF51 a
      "ready for shutdown" message

  ... the nRF51 cuts the power. If no "ready to shutdown" was received
      after a timeout, the power is cut anyway

Closes #623

src/hal/interface/syslink.h

@@ -64,8 +64,10 @@
 #define SYSLINK_OW_READ     0x22
 #define SYSLINK_OW_WRITE    0x23
 
-#define SYSLINK_SYS_GROUP        0x30
-#define SYSLINK_SYS_NRF_VERSION  0x30
+#define SYSLINK_SYS_GROUP            0x30
+#define SYSLINK_SYS_NRF_VERSION      0x30
+#define SYSLINK_SYS_SHUTDOWN_REQUEST 0x31
+#define SYSLINK_SYS_SHUTDOWN_ACK     0x32
 
 typedef struct _SyslinkPacket
 {

src/modules/interface/system.h

@@ -30,6 +30,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+typedef void (*graceful_shutdown_callback_t)();
+
 void systemInit(void);
 bool systemTest(void);
 
@@ -45,4 +47,6 @@ void systemRequestShutdown();
 void systemRequestNRFVersion();
 void systemSyslinkReceive();
 
+bool systemRegisterGracefulShutdownCallback(graceful_shutdown_callback_t cb);
+
 #endif //__SYSTEM_H__

src/modules/src/system.c

@@ -352,17 +352,70 @@ void systemRequestNRFVersion()
   syslinkSendPacket(&slp);
 }
 
+/*
+ * When a module whants to register a callback to be called on shutdown they
+ * call systemRegisterGracefulShutdownCallback(graceful_shutdown_callback_t),
+ * with a function they which to be run at shutdown. We currently support
+ * GRACEFUL_SHUTDOWN_MAX_CALLBACKS number of callbacks to be registred.
+ */
+#define GRACEFUL_SHUTDOWN_MAX_CALLBACKS 5
+static uint8_t graceful_shutdown_callbacks_index;
+static graceful_shutdown_callback_t graceful_shutdown_callbacks[GRACEFUL_SHUTDOWN_MAX_CALLBACKS];
+
+/*
+ * Please take care in your callback, do not take to long time the nrf
+ * will not wait for you, it will shutdown.
+ */
+bool systemRegisterGracefulShutdownCallback(graceful_shutdown_callback_t cb)
+{
+  // To many registered allready! Increase limit if you think you are important
+  // enough!
+  if (graceful_shutdown_callbacks_index >= GRACEFUL_SHUTDOWN_MAX_CALLBACKS) {
+    return false;
+  }
+
+  graceful_shutdown_callbacks[graceful_shutdown_callbacks_index] = cb;
+  graceful_shutdown_callbacks_index += 1;
+
+  return true;
+}
+
+/*
+ * Iterate through all registered shutdown callbacks and call them one after
+ * the other, when all is done, send the ACK back to nrf to allow power off.
+ */
+void systemGracefulShutdown()
+{
+  for (int i = 0; i < graceful_shutdown_callbacks_index; i++) {
+    graceful_shutdown_callback_t callback = graceful_shutdown_callbacks[i];
+
+    callback();
+  }
+
+  SyslinkPacket slp = {
+    .type = SYSLINK_SYS_SHUTDOWN_ACK,
+  };
+
+  syslinkSendPacket(&slp);
+}
+
 void systemSyslinkReceive(SyslinkPacket *slp)
 {
-  if (slp->type == SYSLINK_SYS_NRF_VERSION)
+  switch (slp->type)
   {
+  case SYSLINK_SYS_NRF_VERSION:{
     size_t len = slp->length - 2;
 
     if (sizeof(nrf_version) - 1 <=  len) {
       len = sizeof(nrf_version) - 1;
     }
     memcpy(&nrf_version, &slp->data[0], len);
     DEBUG_PRINT("NRF51 version: %s\n", nrf_version);
+  } break;
+
+  case SYSLINK_SYS_SHUTDOWN_REQUEST:
+    systemGracefulShutdown();
+    break;
   }
 }