[feat][pid] add pid algorithm

common/CMakeLists.txt

@@ -1,17 +1,14 @@
 ################# Add global include #################
 list(APPEND ADD_INCLUDE
-# "${CMAKE_CURRENT_SOURCE_DIR}/libc/inc"
 "${CMAKE_CURRENT_SOURCE_DIR}/ring_buffer"
 "${CMAKE_CURRENT_SOURCE_DIR}/soft_crc"
 "${CMAKE_CURRENT_SOURCE_DIR}/memheap"
-# "${CMAKE_CURRENT_SOURCE_DIR}/libc/inc/arm_gcc"
-# "${CMAKE_CURRENT_SOURCE_DIR}/libc/inc/bits"
-# "${CMAKE_CURRENT_SOURCE_DIR}/libc/inc/sys"
 "${CMAKE_CURRENT_SOURCE_DIR}/misc"
 "${CMAKE_CURRENT_SOURCE_DIR}/list"
 "${CMAKE_CURRENT_SOURCE_DIR}/device"
 "${CMAKE_CURRENT_SOURCE_DIR}/partition"
 "${CMAKE_CURRENT_SOURCE_DIR}/bl_math"
+"${CMAKE_CURRENT_SOURCE_DIR}/pid"
 )
 #######################################################
 
@@ -22,7 +19,6 @@ list(APPEND ADD_INCLUDE
 
 ############## Add current dir source files ###########
 file(GLOB_RECURSE sources
-# "${CMAKE_CURRENT_SOURCE_DIR}/libc/src/*.c"
 "${CMAKE_CURRENT_SOURCE_DIR}/ring_buffer/*.c"
 "${CMAKE_CURRENT_SOURCE_DIR}/soft_crc/*.c"
 "${CMAKE_CURRENT_SOURCE_DIR}/memheap/*.c"
@@ -30,14 +26,11 @@ file(GLOB_RECURSE sources
 "${CMAKE_CURRENT_SOURCE_DIR}/device/*.c"
 "${CMAKE_CURRENT_SOURCE_DIR}/partition/*.c"
 "${CMAKE_CURRENT_SOURCE_DIR}/bl_math/*.c"
+"${CMAKE_CURRENT_SOURCE_DIR}/pid/*.c"
 )
 
 #aux_source_directory(. sources)
 list(APPEND ADD_SRCS  ${sources})
-list(REMOVE_ITEM ADD_SRCS
-"${CMAKE_CURRENT_SOURCE_DIR}/libc/src/strtox.c"
-"${CMAKE_CURRENT_SOURCE_DIR}/libc/src/atox.c"
-"${CMAKE_CURRENT_SOURCE_DIR}/libc/src/stdlib.c")
 #######################################################
 
 ########### Add required/dependent components #########
@@ -55,7 +48,8 @@ list(REMOVE_ITEM ADD_SRCS
 
 ############ Add global compile option ################
 #add components denpend on this component
-# list(APPEND ADD_DEFINITIONS -Dxxx)
+string(TOUPPER ${CHIP} CHIPNAME)
+list(APPEND ADD_DEFINITIONS -D${CHIPNAME})
 #######################################################
 
 ############ Add private compile option ################

common/pid/pid.c

@@ -0,0 +1,68 @@
+/**
+ * @file pid.c
+ * @brief
+ *
+ * Copyright (c) 2021 Bouffalolab team
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ */
+
+#include "pid.h"
+
+void pid_init(pid_alg_t *pid)
+{
+    pid->set_val = 0.0f;
+    pid->out_val = 0.0f;
+
+    pid->last_error = 0.0f;
+    pid->prev_error = 0.0f;
+
+    pid->kp = 3.0f;
+    pid->ki = 0.0f;
+    pid->kd = 0.0f;
+
+    pid->i_error = 0.0f;
+    pid->sum_error = 0.0f;
+
+    pid->max_val = 32;
+    pid->min_val = -32;
+}
+
+// standard pid
+float standard_pid_cal(pid_alg_t *pid, float next_val)
+{
+    pid->set_val = next_val;
+    pid->i_error = pid->set_val - pid->out_val;
+    pid->sum_error += pid->i_error;
+    pid->out_val = pid->kp * pid->i_error + pid->ki * pid->sum_error + pid->kd * (pid->i_error - pid->last_error);
+    pid->last_error = pid->i_error;
+
+    return pid->out_val;
+}
+
+// increment pid
+float increment_pid_cal(pid_alg_t *pid, float next_val)
+{
+    pid->set_val = next_val;
+    pid->i_error = pid->set_val - pid->out_val;
+    float increment = pid->kp * (pid->i_error - pid->prev_error) + pid->ki * pid->i_error + pid->kd * (pid->i_error - 2 * pid->prev_error + pid->last_error);
+    pid->out_val += increment;
+    pid->last_error = pid->prev_error;
+    pid->prev_error = pid->i_error;
+
+    return pid->out_val;
+}

common/pid/pid.h

@@ -0,0 +1,50 @@
+/**
+ * @file pid.h
+ * @brief
+ *
+ * Copyright (c) 2021 Bouffalolab team
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ */
+
+#ifndef __PID_H__
+#define __PID_H__
+
+#include "stdio.h"
+
+typedef struct pid_alg {
+    float set_val;
+    float out_val;
+
+    float kp;
+    float ki;
+    float kd;
+
+    float i_error;
+    float last_error;
+    float prev_error;
+    float sum_error;
+
+    int max_val;
+    int min_val;
+} pid_alg_t;
+
+void pid_init(pid_alg_t *pid);
+float standard_pid_cal(pid_alg_t *pid, float next_val);
+float increment_pid_cal(pid_alg_t *pid, float next_val);
+
+#endif