Inertial Control

I added inertial navigation based on the simulator data. This is an option only available if you compile with Arduino and set
#define INERTIAL_NAV ENABLED
in the APM_Config.h file.

This has been tested for one real flight and did not crash my quad, but consider it very alpha. The quad may be unpredictable at first until the error correction fixes poorly calibrated accels. Be Careful.

Most of the real work is in the inertia file, but the error correction, new variable defines and calibration calls are sprinkled throughout.

The Log should record RAW messages with special debugging values.

ArduCopter/APM_Config.h

@@ -43,6 +43,26 @@
 
 #define ACCEL_ALT_HOLD 0		// disabled by default, work in progress
 
+#define INERTIAL_NAV ENABLED
+
+
+#if INERTIAL_NAV == ENABLED
+	#define ALT_HOLD_P			0.5
+	#define ALT_HOLD_I			0.007
+	#define ALT_HOLD_IMAX		300
+
+	// RATE control
+	#define THROTTLE_P			2		//
+	#define THROTTLE_I			0.5		// Don't edit
+	#define THROTTLE_D			0.0		//
+
+	#define LOITER_P			0.50
+	#define LOITER_I			0.0
+	#define LOITER_RATE_P		12			//
+	#define LOITER_RATE_I		1.0		// Wind control
+	#define LOITER_RATE_D		0.0		// try 2 or 3 for LOITER_RATE 1
+#endif
+
 
 //#define RATE_ROLL_I 	0.18
 //#define RATE_PITCH_I	0.18

ArduCopter/ArduCopter.pde

@@ -831,6 +831,23 @@ static uint32_t condition_start;
 // Updated with the fast loop
 static float G_Dt		= 0.02;
 
+////////////////////////////////////////////////////////////////////////////////
+// Inertial Navigation
+////////////////////////////////////////////////////////////////////////////////
+#if INERTIAL_NAV == ENABLED
+// The rotated accelerometer values
+static Vector3f accels_velocity;
+
+// accels rotated to world frame
+static Vector3f accels_rotated;
+
+// error correction
+static Vector3f speed_error;
+
+// Manage accel drift
+static Vector3f accels_offset;
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////
 // Performance monitoring
 ////////////////////////////////////////////////////////////////////////////////
@@ -979,6 +996,12 @@ static void fast_loop()
 	// --------------------
 	read_AHRS();
 
+	// Inertial Nav
+	// --------------------
+	#if INERTIAL_NAV == ENABLED
+	calc_inertia();
+	#endif
+
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
 	update_yaw_mode();
@@ -2002,6 +2025,11 @@ static void update_altitude()
 
 	// calc error
 	climb_rate_error = (climb_rate_actual - climb_rate) / 5;
+
+	#if INERTIAL_NAV == ENABLED
+	// inertial_nav
+	z_error_correction();
+	#endif
 }
 
 static void update_altitude_est()

ArduCopter/Attitude.pde

@@ -297,7 +297,11 @@ get_nav_throttle(int32_t z_error)
 	int16_t i_hold 	= g.pi_alt_hold.get_i(z_error, .02);
 
 	// calculate rate error
-	z_rate_error 	= z_target_speed - climb_rate;
+	#if INERTIAL_NAV == ENABLED
+	z_rate_error	= z_target_speed - accels_velocity.z;			// calc the speed error
+	#else
+	z_rate_error	= z_target_speed - climb_rate;		// calc the speed error
+	#endif
 
 	// limit the rate
 	output =  constrain(g.pid_throttle.get_pid(z_rate_error, .02), -80, 120);

ArduCopter/Log.pde

@@ -287,16 +287,47 @@ static void Log_Read_GPS()
 							temp8);		// 8 ground course
 }
 
-// Write an raw accel/gyro data packet. Total length : 28 bytes
+#if INERTIAL_NAV == ENABLED
+static void Log_Write_Raw()
+{
+	Vector3f accel = imu.get_accel();
+
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_RAW_MSG);
+
+	DataFlash.WriteLong(get_int(accels_offset.x));
+	DataFlash.WriteLong(get_int(accels_velocity.x));
+	DataFlash.WriteLong(get_int(speed_error.x));
+
+	DataFlash.WriteLong(get_int(accels_offset.z));
+	DataFlash.WriteLong(get_int(accels_velocity.z));
+	DataFlash.WriteLong(get_int(speed_error.z));
+
+	DataFlash.WriteLong(get_int(accel.x));
+	DataFlash.WriteLong(get_int(accel.y));
+	DataFlash.WriteLong(get_int(accel.z));
+
+	DataFlash.WriteByte(END_BYTE);
+}
+
+// Read a raw accel/gyro packet
+static void Log_Read_Raw()
+{
+	float logvar;
+	Serial.printf_P(PSTR("RAW,"));
+	for (int y = 0; y < 9; y++) {
+		logvar = get_float(DataFlash.ReadLong());
+		Serial.print(logvar);
+		Serial.print(", ");
+	}
+	Serial.println(" ");
+}
+#else
 static void Log_Write_Raw()
 {
 	Vector3f gyro = imu.get_gyro();
 	Vector3f accel = imu.get_accel();
-	//Vector3f accel_filt	= imu.get_accel_filtered();
-
-	//gyro *= t7;								// Scale up for storage as long integers
-	//accel *= t7;
-	//accel_filt *= t7;
 
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
@@ -325,6 +356,8 @@ static void Log_Read_Raw()
 	}
 	Serial.println(" ");
 }
+#endif
+
 
 // Write an Current data packet. Total length : 16 bytes
 static void Log_Write_Current()

ArduCopter/inertia.pde

@@ -0,0 +1,114 @@
+#if INERTIAL_NAV == ENABLED
+
+// generates a new location and velocity in space based on inertia
+// Calc 100 hz
+void calc_inertia()
+{
+	// rotate accels based on DCM
+	// --------------------------
+	accels_rotated		= ahrs.get_dcm_matrix() * imu.get_accel();
+	accels_rotated		+= accels_offset;						// skew accels to account for long term error using calibration
+	accels_rotated.z 	+= 9.805;								// remove influence of gravity
+
+	// rising 		= 2
+	// neutral 		= 0
+	// falling 		= -2
+
+
+	// ACC Y POS = going EAST
+	// ACC X POS = going North
+	// ACC Z POS = going DOWN (lets flip this)
+
+	// Integrate accels to get the velocity
+	// ------------------------------------
+	Vector3f temp = accels_rotated * (G_Dt * 100);
+	temp.z = -temp.z;
+	// Temp is changed to world frame and we can use it normaly
+
+	// Integrate accels to get the velocity
+	// ------------------------------------
+	accels_velocity			+= temp;
+}
+
+void z_error_correction()
+{
+	speed_error.z 		= climb_rate - accels_velocity.z;
+	accels_velocity.z	+= speed_error.z * 0.0350;							//speed_correction_z;
+	accels_velocity.z   -= g.pid_throttle.get_integrator() * 0.0045; 		//g.alt_offset_correction; // OK
+	accels_offset.z		-= g.pid_throttle.get_integrator() * 0.000003;		//g.alt_i_correction ; 	// .000002;
+}
+
+void xy_error_correction()
+{
+	// Calculate speed error
+	// ---------------------
+	speed_error.x 		= x_actual_speed - accels_velocity.x;
+	speed_error.y 		= y_actual_speed - accels_velocity.y;
+
+	// correct integrated velocity by speed_error
+	// this number must be small or we will bring back sensor latency
+	// -------------------------------------------
+	accels_velocity.x	+= speed_error.x * 0.0175;								// g.speed_correction_x;
+	accels_velocity.y	+= speed_error.y * 0.0175;
+
+	// Error correct the accels to deal with calibration, drift and noise
+	// ------------------------------------------------------------------
+	accels_velocity.x	-= g.pid_loiter_rate_lon.get_integrator() * 0.007; 		// g.loiter_offset_correction; //.001;
+	accels_velocity.y	-= g.pid_loiter_rate_lat.get_integrator() * 0.007; 		// g.loiter_offset_correction; //.001;
+
+	// update our accel offsets
+	// -------------------------
+	accels_offset.x		-= g.pid_loiter_rate_lon.get_integrator() * 0.000003; 	// g.loiter_i_correction;
+	accels_offset.y		-= g.pid_loiter_rate_lat.get_integrator() * 0.000003; 	// g.loiter_i_correction;
+
+
+	// For developement only
+	// ---------------------
+	if(motors.armed())
+		Log_Write_Raw();
+}
+
+static void calibrate_accels()
+{
+	// sets accels_velocity to 0,0,0
+	zero_accels();
+
+	accels_offset.x = 0;
+	accels_offset.y = 0;
+	accels_offset.z = 0;
+
+	for (int i = 0; i < 200; i++){
+		delay(10);
+		read_AHRS();
+	}
+
+	for (int i = 0; i < 100; i++){
+		delay(10);
+		read_AHRS();
+		calc_inertia();
+		Serial.printf("call accels: %1.5f, %1.5f, %1.5f,\n", accels_rotated.x, accels_rotated.y, accels_rotated.z);
+	}
+
+	accels_velocity /= 100;
+	accels_offset = accels_velocity;
+	zero_accels();
+	calc_inertia();
+
+	Log_Write_Data(25, (float)accels_offset.x);
+	Log_Write_Data(26, (float)accels_offset.y);
+	Log_Write_Data(27, (float)accels_offset.z);
+}
+
+void zero_accels()
+{
+	accels_rotated.x 	= 0;
+	accels_rotated.y 	= 0;
+	accels_rotated.z 	= 0;
+
+	accels_velocity.x 	= 0;
+	accels_velocity.y 	= 0;
+	accels_velocity.z 	= 0;
+}
+
+
+#endif

ArduCopter/motors.pde

@@ -112,6 +112,10 @@ static void init_arm_motors()
 		startup_ground();
 	}
 
+	#if INERTIAL_NAV == ENABLED
+	calibrate_accels();
+	#endif
+
 	#if HIL_MODE != HIL_MODE_ATTITUDE
 		// read Baro pressure at ground -
 		// this resets Baro for more accuracy

ArduCopter/navigation.pde

@@ -64,6 +64,11 @@ static void calc_XY_velocity(){
 	last_longitude 	= g_gps->longitude;
 	last_latitude 	= g_gps->latitude;
 
+	#if INERTIAL_NAV == ENABLED
+	// inertial_nav
+	xy_error_correction();
+	#endif
+
 	/*if(g_gps->ground_speed > 150){
 		float temp = radians((float)g_gps->ground_course/100.0);
 		x_actual_speed = (float)g_gps->ground_speed * sin(temp);
@@ -107,7 +112,15 @@ static void calc_loiter(int x_error, int y_error)
 	}
 #endif
 
+
+	// calculate rate error
+	#if INERTIAL_NAV == ENABLED
+	x_rate_error	= x_target_speed - accels_velocity.x;		// calc the speed error
+	#else
 	x_rate_error	= x_target_speed - x_actual_speed;			// calc the speed error
+	#endif
+
+
 	p				= g.pid_loiter_rate_lon.get_p(x_rate_error);
 	i				= g.pid_loiter_rate_lon.get_i(x_rate_error + x_error, dTnav);
 	d				= g.pid_loiter_rate_lon.get_d(x_error, dTnav);
@@ -138,7 +151,13 @@ static void calc_loiter(int x_error, int y_error)
 	}
 #endif
 
-	y_rate_error	= y_target_speed - y_actual_speed;
+	// calculate rate error
+	#if INERTIAL_NAV == ENABLED
+	y_rate_error	= y_target_speed - accels_velocity.y;		// calc the speed error
+	#else
+	y_rate_error	= y_target_speed - y_actual_speed;			// calc the speed error
+	#endif
+
 	p				= g.pid_loiter_rate_lat.get_p(y_rate_error);
 	i				= g.pid_loiter_rate_lat.get_i(y_rate_error + y_error, dTnav);
 	d				= g.pid_loiter_rate_lat.get_d(y_error, dTnav);