Indi controller

Makefile

@@ -36,7 +36,7 @@ CFLAGS += -DCRAZYFLIE_FW
 ######### Stabilizer configuration ##########
 ## These are set by the platform (see tools/make/platforms/*.mk), can be overwritten here
 ESTIMATOR          ?= any
-CONTROLLER         ?= Any # one of Any, PID, Mellinger
+CONTROLLER         ?= Any # one of Any, PID, Mellinger, INDI
 POWER_DISTRIBUTION ?= stock
 
 #OpenOCD conf
@@ -156,7 +156,7 @@ PROJ_OBJ += crtp_commander_generic.o crtp_localization_service.o
 PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o
 PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o
 PROJ_OBJ += estimator.o estimator_complementary.o
-PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o
+PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o
 PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o
 PROJ_OBJ += estimator_kalman.o kalman_core.o kalman_supervisor.o
 

src/modules/interface/controller.h

@@ -32,6 +32,7 @@ typedef enum {
   ControllerTypeAny,
   ControllerTypePID,
   ControllerTypeMellinger,
+  ControllerTypeINDI,
   ControllerType_COUNT,
 } ControllerType;
 

src/modules/interface/controller_indi.h

@@ -0,0 +1,106 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2016 Bitcraze AB
+ *
+ * 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, in version 3.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * controller_indi.h - INDI Controller Interface
+ */
+#ifndef __CONTROLLER_INDI_H__
+#define __CONTROLLER_INDI_H__
+
+#include "stabilizer_types.h"
+#include "filter.h"
+#include "math3d.h"
+#include "log.h"
+#include "param.h"
+#include "position_controller.h"
+#include "attitude_controller.h"
+
+#define ATTITUDE_UPDATE_DT    (float)(1.0f/ATTITUDE_RATE)
+
+#define PI 3.14159265358979323846f
+
+// these parameters are used in the filtering of the angular acceleration
+#define STABILIZATION_INDI_FILT_CUTOFF 8.0f
+
+// the yaw sometimes requires more filtering
+#define STABILIZATION_INDI_FILT_CUTOFF_R STABILIZATION_INDI_FILT_CUTOFF
+
+// these parameters are used in the filtering of the angular acceleration
+#define STABILIZATION_INDI_G1_P 0.0034724f
+#define STABILIZATION_INDI_G1_Q 0.0052575f
+#define STABILIZATION_INDI_G1_R -0.0015942f
+#define STABILIZATION_INDI_G2_R -0.11281f
+#define STABILIZATION_INDI_REF_ERR_P 3.57f
+#define STABILIZATION_INDI_REF_ERR_Q 3.57f
+#define STABILIZATION_INDI_REF_ERR_R 3.57f
+#define STABILIZATION_INDI_REF_RATE_P 14.0f
+#define STABILIZATION_INDI_REF_RATE_Q 14.0f
+#define STABILIZATION_INDI_REF_RATE_R 14.0f
+#define STABILIZATION_INDI_ACT_DYN_P 0.03149f
+#define STABILIZATION_INDI_ACT_DYN_Q 0.03149f
+#define STABILIZATION_INDI_ACT_DYN_R 0.03149f
+
+/**
+ * @brief angular rates
+ * @details Units: rad/s */
+struct FloatRates {
+  float p; ///< in rad/s
+  float q; ///< in rad/s
+  float r; ///< in rad/s
+};
+
+struct ReferenceSystem {
+  float err_p;
+  float err_q;
+  float err_r;
+  float rate_p;
+  float rate_q;
+  float rate_r;
+};
+
+struct IndiVariables {
+  float thrust;
+  struct FloatRates angular_accel_ref;
+  struct FloatRates du;
+  struct FloatRates u_in;
+  struct FloatRates u_act_dyn;
+  float rate_d[3];
+
+  Butterworth2LowPass u[3];
+  Butterworth2LowPass rate[3];
+  struct FloatRates g1;
+  float g2;
+
+  struct ReferenceSystem reference_acceleration;
+  struct FloatRates act_dyn;
+  float filt_cutoff;
+  float filt_cutoff_r;
+};
+
+void controllerINDIInit(void);
+bool controllerINDITest(void);
+void controllerINDI(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+
+#endif //__CONTROLLER_INDI_H__

src/modules/interface/controller_mellinger.h

@@ -21,7 +21,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * controller_pid.h - Mellinger Controller Interface
+ * controller_mellinger.h - Mellinger Controller Interface
  */
 #ifndef __CONTROLLER_MELLINGER_H__
 #define __CONTROLLER_MELLINGER_H__

src/modules/src/controller.c

@@ -5,6 +5,7 @@
 #include "controller.h"
 #include "controller_pid.h"
 #include "controller_mellinger.h"
+#include "controller_indi.h"
 
 #define DEFAULT_CONTROLLER ControllerTypePID
 static ControllerType currentController = ControllerTypeAny;
@@ -22,6 +23,7 @@ static ControllerFcns controllerFunctions[] = {
   {.init = 0, .test = 0, .update = 0, .name = "None"}, // Any
   {.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
   {.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
+  {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
 };
 
 
@@ -65,4 +67,4 @@ void controller(control_t *control, setpoint_t *setpoint, const sensorData_t *se
 
 const char* controllerGetName() {
   return controllerFunctions[currentController].name;
-}
+}