Implement ASSERT() to make debugging easier (#476)

* Initial cut of ASSERT() implementation

Makefile

@@ -360,6 +360,7 @@ COMMON_SRC = \
             build/build_config.c \
             build/debug.c \
             build/version.c \
+            build/assert.c \
             $(TARGET_DIR_SRC) \
             main.c \
             fc/mw.c \

src/main/build/assert.c

@@ -0,0 +1,52 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "stdbool.h"
+#include "stdint.h"
+
+#include "platform.h"
+
+#if defined(USE_ASSERT)
+char * assertFailureFile = 0L;
+int assertFailureLine = 0;
+
+void assertFailed2(const char * file, int line)
+{
+    if (!assertFailureLine) {
+        assertFailureFile = (char *)file;
+        assertFailureLine = line;
+    }
+#if defined(USE_ASSERT_STOP)
+    while(1) {
+        __asm("BKPT #0\n") ; // Break into the debugger
+    }
+#endif
+}
+
+void assertFailed1(int line)
+{
+    if (!assertFailureLine) {
+        assertFailureFile = 0L;
+        assertFailureLine = line;
+    }
+#if defined(USE_ASSERT_STOP)
+    while(1) {
+        __asm("BKPT #0\n") ; // Break into the debugger
+    }
+#endif
+}
+#endif

src/main/build/assert.h

@@ -0,0 +1,36 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#if defined(USE_ASSERT)
+extern char * assertFailureFile;
+extern int assertFailureLine;
+
+extern void assertFailed2(const char * file, int line);
+extern void assertFailed1(int line);
+#endif
+
+#if defined(USE_ASSERT) && defined(USE_ASSERT_CHECK)
+    #if defined(USE_ASSERT_FULL)
+        #define ASSERT(expr)        if (expr) { } else assertFailed2(__FILE__, __LINE__)
+    #else
+        #define ASSERT(expr)        if (expr) { } else assertFailed1(__LINE__)
+    #endif
+#else
+    #define ASSERT(expr)        while (0) { }
+#endif

src/main/drivers/exti.c

@@ -3,6 +3,7 @@
 #include <string.h>
 
 #include "platform.h"
+#include "build/assert.h"
 
 #include "nvic.h"
 #include "io_impl.h"
@@ -72,6 +73,10 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
     chIdx = IO_GPIOPinIdx(io);
     if(chIdx < 0)
         return;
+
+    // we have only 16 extiChannelRecs
+    ASSERT(chIdx < 16);
+
     extiChannelRec_t *rec = &extiChannelRecs[chIdx];
     int group = extiGroups[chIdx];
 
@@ -117,6 +122,10 @@ void EXTIRelease(IO_t io)
     chIdx = IO_GPIOPinIdx(io);
     if(chIdx < 0)
         return;
+
+    // we have only 16 extiChannelRecs
+    ASSERT(chIdx < 16);
+
     extiChannelRec_t *rec = &extiChannelRecs[chIdx];
     rec->handler = NULL;
 }

src/main/drivers/io.c

@@ -5,6 +5,7 @@
 #include "rcc.h"
 
 #include "target.h"
+#include "build/assert.h"
 
 // io ports defs are stored in array by index now
 struct ioPortDef_s {
@@ -69,6 +70,10 @@ const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {
 
 ioRec_t* IO_Rec(IO_t io)
 {
+    ASSERT(io != NULL);
+    ASSERT((ioRec_t*)io >= &ioRecs[0]);
+    ASSERT((ioRec_t*)io < &ioRecs[DEFIO_IO_USED_COUNT]);
+
     return io;
 }
 

src/main/io/serial_cli.c

@@ -27,6 +27,7 @@
 
 #include "build/version.h"
 #include "build/build_config.h"
+#include "build/assert.h"
 
 #include "scheduler/scheduler.h"
 
@@ -108,6 +109,10 @@ static serialPort_t *cliPort;
 static bufWriter_t *cliWriter;
 static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 16];
 
+#if defined(USE_ASSERT)
+static void cliAssert(char *cmdline);
+#endif
+
 static void cliAux(char *cmdline);
 static void cliRxFail(char *cmdline);
 static void cliAdjustmentRange(char *cmdline);
@@ -260,6 +265,9 @@ typedef struct {
 // should be sorted a..z for bsearch()
 const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
+#if defined(USE_ASSERT)
+    CLI_COMMAND_DEF("assert", "", NULL, cliAssert),
+#endif
     CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
 #ifdef LED_STRIP
     CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
@@ -994,6 +1002,25 @@ static void cliRxFail(char *cmdline)
     }
 }
 
+#if defined(USE_ASSERT)
+static void cliAssert(char *cmdline)
+{
+    UNUSED(cmdline);
+
+    if (assertFailureLine) {
+        if (assertFailureFile) {
+            cliPrintf("Assertion failed at line %d, file %s\r\n", assertFailureLine, assertFailureFile);
+        }
+        else {
+            cliPrintf("Assertion failed at line %d\r\n", assertFailureLine);
+        }
+    }
+    else {
+        cliPrintf("No assert() failed\r\n");
+    }
+}
+#endif
+
 static void cliAux(char *cmdline)
 {
     int i, val = 0;

src/main/main.c

@@ -23,6 +23,7 @@
 
 #include "build/atomic.h"
 #include "build/build_config.h"
+#include "build/assert.h"
 #include "build/debug.h"
 
 #include "common/axis.h"

src/main/target/CC3D/target.h

@@ -162,7 +162,7 @@
 
 #define NAV
 //#define NAV_AUTO_MAG_DECLINATION
-#define NAV_GPS_GLITCH_DETECTION
+//#define NAV_GPS_GLITCH_DETECTION
 #define NAV_MAX_WAYPOINTS       30
 
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
@@ -208,6 +208,11 @@
 #define MAX_PWM_OUTPUT_PORTS    11
 
 // DEBUG
+#define USE_ASSERT          // include assertion support code
+#define USE_ASSERT_FULL     // Provide file information
+//#define USE_ASSERT_STOP   // stop on failed assertion
+//#define USE_ASSERT_CHECK    // include assertion check code (should in general a per-file define)
+
 //#define HIL
 //#define USE_FAKE_MAG
 //#define USE_FAKE_BARO