Incremental Nonlinear Dynamic Inversion (INDI) position controller for the Crazyflie

Makefile

@@ -168,7 +168,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 controller_indi.o
+PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o position_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_indi.h

@@ -33,6 +33,7 @@
 #include "param.h"
 #include "position_controller.h"
 #include "attitude_controller.h"
+#include "position_controller_indi.h"
 
 #define ATTITUDE_UPDATE_DT    (float)(1.0f/ATTITUDE_RATE)
 
@@ -43,13 +44,13 @@
 #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_G1_P 0.0066146f
+#define STABILIZATION_INDI_G1_Q 0.0052125f
+#define STABILIZATION_INDI_G1_R -0.001497f
+#define STABILIZATION_INDI_G2_R 0.000043475f
+#define STABILIZATION_INDI_REF_ERR_P 24.0f
+#define STABILIZATION_INDI_REF_ERR_Q 24.0f
+#define STABILIZATION_INDI_REF_ERR_R 24.0f
 #define STABILIZATION_INDI_REF_RATE_P 14.0f
 #define STABILIZATION_INDI_REF_RATE_Q 14.0f
 #define STABILIZATION_INDI_REF_RATE_R 14.0f

src/modules/interface/position_controller_indi.h

@@ -0,0 +1,86 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * position_controller_indi.h - INDI Controller Interface
+ */
+
+#ifndef __POSITION_CONTROLLER_INDI_H__
+#define __POSITION_CONTROLLER_INDI_H__
+
+// Thrust command (in motor units)
+#define MIN_THRUST  0
+#define MAX_THRUST  60000
+
+// Cutoff frequency used in the filtering 
+#define POSITION_INDI_FILT_CUTOFF 8.0f
+
+#include "controller_indi.h"
+#include "stabilizer_types.h"
+#include "filter.h"
+#include "math3d.h"
+#include "log.h"
+#include "param.h"
+
+struct Vectr {
+  float x; 
+  float y; 
+  float z;
+};
+
+struct Angles {
+  float phi; 
+  float theta;
+  float psi;
+};
+
+struct IndiOuterVariables {
+
+  Butterworth2LowPass ddxi[3];
+  Butterworth2LowPass ang[3];
+  Butterworth2LowPass thr[3];
+
+  float filt_cutoff;
+  float act_dyn_posINDI;
+
+  struct Vectr linear_accel_ref;  
+  struct Vectr linear_accel_err;
+  struct Vectr linear_accel_s;    // acceleration sensed
+  struct Vectr linear_accel_f;    // acceleration filtered
+  struct Vectr linear_accel_ft;   // acceleration filtered transformed to NED 
+  struct Angles attitude_s;       // attitude senssed (here estimated)
+  struct Angles attitude_f;       // attitude filtered
+  struct Angles attitude_c;       // attitude commanded to the inner loop
+  float phi_tilde;                // roll angle increment
+  float theta_tilde;              // pitch angle increment
+  float T_tilde;                  // thrust increment
+  float T_inner;                  // thrust to inner INDI
+  float T_inner_f;
+  float T_incremented;
+};
+
+void positionControllerINDIInit(void);
+void positionControllerINDI(const sensorData_t *sensors, 
+                            const state_t *state, 
+                            vector_t *refOuterINDI);
+
+#endif //__POSITION_CONTROLLER_INDI_H__

src/modules/src/controller_indi.c

@@ -31,15 +31,20 @@ static float bound_control_input = 32000.0f;
 static attitude_t attitudeDesired;
 static attitude_t rateDesired;
 static float actuatorThrust;
-static float roll_kp = 6.0f;
-static float pitch_kp = 6.0f;
-static float yaw_kp = 6.0f;
+static float roll_kp = 5.0f;
+static float pitch_kp = 5.0f;
+static float yaw_kp = 5.0f;
+
+static float attYawError; 
 
 static float r_roll;
 static float r_pitch;
 static float r_yaw;
 static float accelz;
 
+static vector_t refOuterINDI;		// Reference values from outer loop INDI
+static float outerLoopActive; 		// if 1, outer loop INDI is activated
+
 static struct IndiVariables indi = {
 		.g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
 		.g2 = STABILIZATION_INDI_G2_R,
@@ -119,6 +124,7 @@ void controllerINDIInit(void)
 
 	attitudeControllerInit(ATTITUDE_UPDATE_DT);
 	positionControllerInit();
+	positionControllerINDIInit();
 }
 
 bool controllerINDITest(void)
@@ -149,7 +155,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 		}
 	}
 
-	if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
+	if (RATE_DO_EXECUTE(POSITION_RATE, tick) && !outerLoopActive) {
 		positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
 	}
 
@@ -158,13 +164,33 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 	 */
 	if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
 
+		// Call outer loop INDI (position controller)
+		if (outerLoopActive) {
+			positionControllerINDI(sensors, state, &refOuterINDI);
+		}
+
 		// Switch between manual and automatic position control
 		if (setpoint->mode.z == modeDisable) {
-			actuatorThrust = setpoint->thrust;
+			if (outerLoopActive) {
+				// INDI position controller active, INDI attitude controller becomes inner loop
+				actuatorThrust = refOuterINDI.z;
+			}
+			else {
+				// INDI position controller not active, INDI attitude controller is main loop
+				actuatorThrust = setpoint->thrust;
+			}
 		}
 		if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
-			attitudeDesired.roll = setpoint->attitude.roll;
-			attitudeDesired.pitch = setpoint->attitude.pitch;
+			if (outerLoopActive) {
+				// INDI position controller active, INDI attitude controller becomes inner loop
+				attitudeDesired.roll = refOuterINDI.x;
+				attitudeDesired.pitch = refOuterINDI.y;
+			}
+			else {
+				// INDI position controller not active, INDI attitude controller is main loop
+				attitudeDesired.roll = setpoint->attitude.roll;
+				attitudeDesired.pitch = setpoint->attitude.pitch;
+			}
 		}
 
 //	    attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
@@ -173,7 +199,16 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 
 		rateDesired.roll = roll_kp*(attitudeDesired.roll - state->attitude.roll);
 		rateDesired.pitch = pitch_kp*(attitudeDesired.pitch - state->attitude.pitch);
-		rateDesired.yaw = yaw_kp*(attitudeDesired.yaw - state->attitude.yaw);
+		//rateDesired.yaw = yaw_kp*(attitudeDesired.yaw - state->attitude.yaw);
+		attYawError = attitudeDesired.yaw - state->attitude.yaw;		
+		if (attYawError > 180.0f) {
+			attYawError = attYawError - 360.0f;
+		}
+		else if (attYawError < -180.0f) {
+			attYawError = attYawError + 360.0f;
+		}
+		rateDesired.yaw = yaw_kp*attYawError;
+
 
 		// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
 		// value. Also reset the PID to avoid error buildup, which can lead to unstable
@@ -228,14 +263,11 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 		float attitude_error_q = radians(rateDesired.pitch) - stateAttitudeRatePitch;
 		float attitude_error_r = radians(rateDesired.yaw) - stateAttitudeRateYaw;
 
-		indi.angular_accel_ref.p = indi.reference_acceleration.err_p * attitude_error_p
-				- indi.reference_acceleration.rate_p * body_rates.p;
+		indi.angular_accel_ref.p = indi.reference_acceleration.err_p * attitude_error_p;
 
-		indi.angular_accel_ref.q = indi.reference_acceleration.err_q * attitude_error_q
-				- indi.reference_acceleration.rate_q * body_rates.q;
+		indi.angular_accel_ref.q = indi.reference_acceleration.err_q * attitude_error_q;
 
-		indi.angular_accel_ref.r = indi.reference_acceleration.err_r * attitude_error_r
-				- indi.reference_acceleration.rate_r * body_rates.r;
+		indi.angular_accel_ref.r = indi.reference_acceleration.err_r * attitude_error_r;
 
 		/*
 		 * 5. Update the For each axis: delta_command = 1/control_effectiveness * (angular_acceleration_reference – angular_acceleration)
@@ -247,7 +279,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 		//(they have significant inertia, see the paper mentioned in the header for more explanation)
 		indi.du.p = 1.0f / indi.g1.p * (indi.angular_accel_ref.p - indi.rate_d[0]);
 		indi.du.q = 1.0f / indi.g1.q * (indi.angular_accel_ref.q - indi.rate_d[1]);
-		indi.du.r = 1.0f / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] + indi.g2 * indi.du.r);
+		indi.du.r = 1.0f / (indi.g1.r - indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] - indi.g2 * indi.du.r);
 
 
 		/*
@@ -299,6 +331,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 	control->roll = indi.u_in.p;
 	control->pitch = indi.u_in.q;
 	control->yaw  = indi.u_in.r;
+
 }
 
 PARAM_GROUP_START(ctrlINDI)
@@ -322,6 +355,7 @@ PARAM_ADD(PARAM_FLOAT, act_dyn_q, &indi.act_dyn.q)
 PARAM_ADD(PARAM_FLOAT, act_dyn_r, &indi.act_dyn.r)
 PARAM_ADD(PARAM_FLOAT, filt_cutoff, &indi.filt_cutoff)
 PARAM_ADD(PARAM_FLOAT, filt_cutoff_r, &indi.filt_cutoff_r)
+PARAM_ADD(PARAM_FLOAT, outerLoopActive, &outerLoopActive)
 PARAM_GROUP_STOP(ctrlINDI)
 
 LOG_GROUP_START(ctrlINDI)