Z dampener

Integrated Aurelio R. Ramos' Z dampening code.

ArduCopter/APM_Config.h

@@ -39,9 +39,7 @@
 	CH7_SAVE_WP
 	*/
 
-#define ACCEL_ALT_HOLD 0		// disabled by default, work in progress
-#define ACCEL_ALT_HOLD_GAIN 20.0
-// ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode
+#define ACCEL_ALT_HOLD 1		// disabled by default, work in progress
 
 // See the config.h and defines.h files for how to set this up!
 //

ArduCopter/ArduCopter.pde

@@ -246,14 +246,6 @@ static const char* flight_mode_strings[] = {
 			8	TBD
 */
 
-// test
-#if ACCEL_ALT_HOLD == 1
-Vector3f accels_rot;
-static int		accels_rot_count;
-static float 	accels_rot_sum;
-static float 	alt_hold_gain = ACCEL_ALT_HOLD_GAIN;
-#endif
-
 // temp
 static int y_actual_speed;
 static int y_rate_error;
@@ -1070,13 +1062,14 @@ void update_throttle_mode(void)
 				g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle);
 			#else
 				angle_boost = get_angle_boost(g.throttle_cruise);
+
 				if(manual_boost != 0){
 					//remove alt_hold_velocity when implemented
-					g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + alt_hold_velocity();
+					g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping();
 					// reset next_WP.alt
 					next_WP.alt = max(current_loc.alt, 100);
 				}else{
-					g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + alt_hold_velocity();
+					g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping();
 				}
 
 			#endif

ArduCopter/Attitude.pde

@@ -176,36 +176,6 @@ get_nav_yaw_offset(int yaw_input, int reset)
 	}
 }
 
-static int alt_hold_velocity()
-{
-	#if ACCEL_ALT_HOLD == 1
-		Vector3f accel_filt;
-		float error;
-
-		// subtract filtered Accel
-		error		= abs(next_WP.alt - current_loc.alt) - 25;
-		error		= min(error, 50.0);
-		error		= max(error, 0.0);
-		error		= 1 - (error/ 50.0);
-
-		accel_filt	= imu.get_accel_filtered();
-		accels_rot 	= dcm.get_dcm_matrix() * imu.get_accel_filtered();
-		int output = (accels_rot.z + 9.81) * alt_hold_gain * error;   // alt_hold_gain  = 12
-
-		//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
-		return constrain(output, -70, 70);
-
-// fast rise
-//s: -17.6241, g:0.0000, e:1.0000, o:0
-//s: -18.4990, g:0.0000, e:1.0000, o:0
-//s: -19.3193, g:0.0000, e:1.0000, o:0
-//s: -13.1310, g:47.8700, e:1.0000, o:-158
-
-	#else
-		return 0;
-	#endif
-}
-
 static int get_angle_boost(int value)
 {
 	float temp = cos_pitch_x * cos_roll_x;
@@ -213,3 +183,101 @@ static int get_angle_boost(int value)
 	return (int)(temp * value);
 }
 
+// Accelerometer Z dampening by Aurelio R. Ramos
+// ---------------------------------------------
+
+#if ACCEL_ALT_HOLD == 1
+
+// contains G and any other DC offset
+static float estimatedAccelOffset = 0;
+
+// state
+static float synVelo = 0;
+static float synPos = 0;
+static float synPosFiltered = 0;
+static float posError = 0;
+static float prevSensedPos = 0;
+
+// tuning for dead reckoning
+static float synPosP = 25 * G_Dt;
+static float synPosI = 100 * G_Dt;
+static float synVeloP = 1.5 * G_Dt;
+static float maxVeloCorrection = 5 * G_Dt;
+static float maxSensedVelo = 1;
+static float synPosFilter = 0.13; // lower to filter more. should approximate sensor filtering
+
+#define NUM_G_SAMPLES 200
+
+// Z damping term.
+static float fullDampP = 0.200;
+
+float get_world_Z_accel()
+{
+	Vector3f accels_rot = dcm.get_dcm_matrix() * imu.get_accel();
+	return accels_rot.z;
+}
+
+
+static void init_z_damper()
+{
+	estimatedAccelOffset = 0;
+	for (int i = 0; i < NUM_G_SAMPLES; i++){
+		estimatedAccelOffset += get_world_Z_accel();
+	}
+	estimatedAccelOffset /= (float)NUM_G_SAMPLES;
+}
+
+float dead_reckon_Z(float sensedPos, float sensedAccel)
+{
+	// the following algorithm synthesizes position and velocity from
+	// a noisy altitude and accelerometer data.
+
+	// synthesize uncorrected velocity by integrating acceleration
+	synVelo += (sensedAccel - estimatedAccelOffset) * G_Dt;
+
+	// synthesize uncorrected position by integrating uncorrected velocity
+	synPos += synVelo * G_Dt;
+
+	// filter synPos, the better this filter matches the filtering and dead time
+	// of the sensed position, the less the position estimate will lag.
+	synPosFiltered = synPosFiltered * (1 - synPosFilter) + synPos * synPosFilter;
+
+	// calculate error against sensor position
+	posError = sensedPos - synPosFiltered;
+
+	// correct altitude
+	synPos += synPosP * posError;
+
+	// correct integrated velocity by posError
+	synVelo = synVelo + constrain(posError, -maxVeloCorrection, maxVeloCorrection) * synPosI;
+
+	// correct integrated velocity by the sensed position delta in a small proportion
+	// (i.e., the low frequency of the delta)
+	float sensedVelo = (sensedPos - prevSensedPos) / G_Dt;
+	synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP;
+
+	prevSensedPos = sensedPos;
+	return synVelo;
+}
+
+static int get_z_damping()
+{
+	float sensedAccel = get_world_Z_accel();
+	float sensedPos = current_loc.alt / 100.0;
+
+	float synVelo = dead_reckon_Z(sensedPos, sensedAccel);
+	return constrain(fullDampP * synVelo * (-1), -300, 300);
+}
+
+#else
+
+static int get_z_damping()
+{
+	return 0;
+}
+
+#endif
+
+
+
+

ArduCopter/motors.pde

@@ -53,6 +53,12 @@ static void arm_motors()
 					// --------------------
 					init_simple_bearing();
 
+					// init the Z damopener
+					// --------------------
+					#if ACCEL_ALT_HOLD == 1
+					init_z_damper();
+					#endif
+
 					// Reset home position
 					// ----------------------
 					if(home_is_set)