OP-1211 time_measurement_helper

flight/modules/Stabilization/stabilization.c

@@ -65,6 +65,11 @@
 #include "relay_tuning.h"
 
 // Private constants
+#define UPDATE_EXPECTED     (1.0f / 666.0f)
+#define UPDATE_MIN          1.0e-6f
+#define UPDATE_MAX          1.0f
+#define UPDATE_ALPHA        1.0e-3f
+
 #define MAX_QUEUE_SIZE      1
 
 #if defined(PIOS_STABILIZATION_STACK_SIZE)
@@ -194,7 +199,9 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
 static void stabilizationTask(__attribute__((unused)) void *parameters)
 {
     UAVObjEvent ev;
-    uint32_t timeval = PIOS_DELAY_GetRaw();
+    PiOSDeltatimeConfig timeval;
+
+    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 
     ActuatorDesiredData actuatorDesired;
     StabilizationDesiredData stabDesired;
@@ -226,8 +233,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
             continue;
         }
 
-        dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
-        timeval = PIOS_DELAY_GetRaw();
+        dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
 
         FlightStatusGet(&flightStatus);
         StabilizationDesiredGet(&stabDesired);

flight/pios/common/pios_deltatime.c

@@ -0,0 +1,68 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DELTATIME time measurement Functions
+ * @brief PiOS Delay functionality
+ * @{
+ *
+ * @file       pios_deltatime.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Settings functions header
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <pios.h>
+
+#ifdef PIOS_INCLUDE_DELTATIME
+
+void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha)
+{
+    PIOS_Assert(config);
+    config->average = average;
+    config->min     = min;
+    config->max     = max;
+    config->alpha   = alpha;
+    config->last    = PIOS_DELAY_GetRaw();
+};
+
+
+float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config)
+{
+    PIOS_Assert(config);
+    float dT = PIOS_DELAY_DiffuS(config->last) * 1.0e-6f;
+    config->last = PIOS_DELAY_GetRaw();
+    if (dT < config->min) {
+        dT = config->min;
+    }
+    if (dT > config->max) {
+        dT = config->max;
+    }
+    config->average = config->average * (1.0f - config->alpha) + dT * config->alpha;
+    return config->average;
+}
+
+
+#endif // PIOS_INCLUDE_DELTATIME
+
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_deltatime.h

@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DELTATIME time measurement Functions
+ * @brief PiOS Delay functionality
+ * @{
+ *
+ * @file       pios_deltatime.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Settings functions header
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_DELTATIME_H
+#define PIOS_DELTATIME_H
+
+struct PiOSDeltatimeConfigStruct {
+    uint32_t last;
+    float    average;
+    float    min;
+    float    max;
+    float    alpha;
+};
+typedef struct PiOSDeltatimeConfigStruct PiOSDeltatimeConfig;
+
+/* Public Functions */
+void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha);
+
+float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config);
+
+#endif /* PIOS_DELTATIME_H */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/pios.h

@@ -104,6 +104,10 @@
 #include <pios_delay.h>
 #endif
 
+#ifdef PIOS_INCLUDE_DELTATIME
+#include <pios_deltatime.h>
+#endif
+
 #ifdef PIOS_INCLUDE_INITCALL
 #include "pios_initcall.h"
 #endif

flight/pios/pios_sim_posix.h

@@ -79,6 +79,7 @@ extern void PIOS_LED_Init(void);
 #include <pios_wdg.h>
 #include <pios_debug.h>
 #include <pios_debuglog.h>
+#include <pios_deltatime.h>
 #include <pios_crc.h>
 #include <pios_rcvr.h>
 #include <pios_flash.h>

flight/targets/boards/coptercontrol/firmware/inc/pios_config.h

@@ -49,6 +49,7 @@
 
 /* PIOS system functions */
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 #define PIOS_INCLUDE_INITCALL
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_TASK_MONITOR

flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h

@@ -32,6 +32,7 @@
 /* Enable/Disable PiOS Modules */
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_FREERTOS
 #define PIOS_INCLUDE_TASK_MONITOR

flight/targets/boards/revolution/firmware/inc/pios_config.h

@@ -49,6 +49,7 @@
 
 /* PIOS system functions */
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 #define PIOS_INCLUDE_INITCALL
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_TASK_MONITOR

flight/targets/boards/revolution/firmware/inc/pios_config_sim.h

@@ -32,6 +32,7 @@
 /* Enable/Disable PiOS Modules */
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_FREERTOS

flight/targets/boards/revoproto/firmware/inc/pios_config.h

@@ -49,6 +49,7 @@
 
 /* PIOS system functions */
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 #define PIOS_INCLUDE_INITCALL
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_TASK_MONITOR

flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h

@@ -1,4 +1,5 @@
 /**
+   #define PIOS_INCLUDE_DELTATIME
  ******************************************************************************
  *
  * @file       pios_config.h
@@ -32,6 +33,7 @@
 /* Enable/Disable PiOS Modules */
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_FREERTOS

flight/targets/boards/simposix/firmware/Makefile

@@ -101,6 +101,7 @@ SRC += $(MATHLIB)/pid.c
 SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
 SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c
 SRC += $(PIOSCORECOMMON)/pios_debuglog.c
+SRC += $(PIOSCORECOMMON)/pios_deltatime.c
 
 ## PIOS Hardware
 include $(PIOS)/posix/library.mk

flight/targets/boards/simposix/firmware/inc/pios_config.h

@@ -41,6 +41,7 @@
 /* Enable/Disable PiOS Modules */
 // #define PIOS_INCLUDE_ADC
 #define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_DELTATIME
 // #define PIOS_INCLUDE_I2C
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_LED

make/apps-defs.mk

@@ -82,6 +82,7 @@ SRC += $(PIOSCOMMON)/pios_crc.c
 SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c
 SRC += $(PIOSCOMMON)/pios_flash_jedec.c
 SRC += $(PIOSCOMMON)/pios_debuglog.c
+SRC += $(PIOSCOMMON)/pios_deltatime.c
 SRC += $(PIOSCOMMON)/pios_rcvr.c
 SRC += $(PIOSCOMMON)/pios_rfm22b.c
 SRC += $(PIOSCOMMON)/pios_rfm22b_com.c