Configuration

iNavFlight · Dec 2, 2020 · 4f3abed · 4f3abed
1 parent fa077fa
commit 4f3abed
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 2 deletions.
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
@@ -115,7 +115,8 @@
 #define PG_OSD_LAYOUTS_CONFIG 1025
 #define PG_SAFE_HOME_CONFIG 1026
 #define PG_DJI_OSD_CONFIG 1027
-#define PG_INAV_END 1027
+#define PG_PROGRAMMING_PID 1028
+#define PG_INAV_END 1028
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
@@ -28,11 +28,52 @@ FILE_COMPILE_FOR_SIZE
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
 
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "programming/pid.h"
 
+EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
+
+PG_REGISTER_ARRAY_WITH_RESET_FN(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids, PG_PROGRAMMING_PID, 1);
+
+void pgResetFn_programmingPids(programmingPid_t *instance)
+{
+    for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        RESET_CONFIG(programmingPid_t, &instance[i],
+            .enabled = 0,
+            .setpoint = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .measurement = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .gains = {
+                .P = 0,
+                .I = 0,
+                .D = 0,
+                .FF = 0,
+            }
+        );
+    }
+}
+
 void programmingPidUpdateTask(timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
     //Dummy
 }
 
+void programmingPidInit(void)
+{
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        navPidInit(
+
+        );
+    }
+}
+
 #endif
diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
@@ -27,4 +27,26 @@
 #include "config/parameter_group.h"
 #include "common/time.h"
 
-void programmingPidUpdateTask(timeUs_t currentTimeUs);
+#include "programming/logic_condition.h"
+#include "common/axis.h"
+#include "flight/pid.h"
+#include "navigation/navigation.h"
+
+#define MAX_PROGRAMMING_PID_COUNT 2
+
+typedef struct programmingPid_s {
+    uint8_t enabled;
+    logicOperand_t setpoint;
+    logicOperand_t measurement;
+    pid8_t gains;
+} programmingPid_t;
+
+PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
+
+typedef struct programmingPidState_s {
+    pidController_t controller;
+    logicOperand_t setpoint;
+} programmingPidState_t;
+
+void programmingPidUpdateTask(timeUs_t currentTimeUs);
+void programmingPidInit(void);