ardupilot/ArduCopter/inertia.pde

#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
	accels_velocity		+= temp;

	// Integrate velocity to get the Position
	// ------------------------------------
	accels_position		+= accels_velocity * G_Dt;

	/*
		current_loc.lng += accels_velocity.x * G_Dt;
		current_loc.lat += accels_velocity.y * G_Dt;
		current_loc.alt += accels_velocity.z * G_Dt;
	*/
}

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;

	// Calculate position error
	// ------------------------
	//position_error.x	= accels_position.x - current_loc.lng;
	//position_error.y	= accels_position.y - current_loc.lat;

	// 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.03;								// g.speed_correction_x;
	accels_velocity.y	+= speed_error.y * 0.03;

	// Error correct the accels to deal with calibration, drift and noise
	// ------------------------------------------------------------------
	//accels_position.x	-= position_error.x * 0.08; 		// g.loiter_offset_correction; //.001;
	//accels_position.y	-= position_error.y * 0.08; 		// g.loiter_offset_correction; //.001;

	accels_position.x = 0;
	accels_position.y = 0;
}

void z_error_correction()
{
	// Calculate speed error
	// ---------------------
	speed_error.z 		= climb_rate - accels_velocity.z;
	//position_error.z	= accels_position.z - current_loc.alt;

	// correct integrated velocity by speed_error
	// this number must be small or we will bring back sensor latency
	// -------------------------------------------
	accels_velocity.z	+= speed_error.z * 0.0350;							//speed_correction_z;

	// ------------------------------------------------------------------
	//accels_position.z   -= position_error.z * 0.006; 		//g.alt_offset_correction; // OK

	accels_position.z = 0;

	// For developement only
	// ---------------------
	if(motors.armed())
		Log_Write_Raw();
}

#endif