X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_flight.c;h=92c955fbfb4dfc53c4101cad6514cc96cf2b6b37;hp=e91a5daa74c97cc77f03b6d3030657c6f061a45e;hb=144db05f6b286a0450d486f69ce192632a2c0656;hpb=17611788aadc9460287145a340a7c18bf63766aa

diff --git a/src/ao_flight.c b/src/ao_flight.c
index e91a5daa..92c955fb 100644
--- a/src/ao_flight.c
+++ b/src/ao_flight.c
@@ -48,6 +48,7 @@ __pdata int16_t			ao_interval_max_pres;
 
 __data uint8_t ao_flight_adc;
 __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres;
+__pdata int16_t ao_accel_2g;
 
 /* Accelerometer calibration
  *
@@ -72,19 +73,18 @@ __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres;
 
 #define GRAVITY 9.80665
 /* convert m/s to velocity count */
-#define VEL_MPS_TO_COUNT(mps) ((int32_t) (((mps) / GRAVITY) * ACCEL_G * 100))
+#define VEL_MPS_TO_COUNT(mps) (((int32_t) (((mps) / GRAVITY) * (AO_HERTZ/2))) * (int32_t) ao_accel_2g)
 
 #define ACCEL_G		265
-#define ACCEL_ZERO_G	16000
-#define ACCEL_NOSE_UP	(ACCEL_G * 2 /3)
-#define ACCEL_BOOST	ACCEL_G * 2
+#define ACCEL_NOSE_UP	(ao_accel_2g / 4)
+#define ACCEL_BOOST	ao_accel_2g
 #define ACCEL_INT_LAND	(ACCEL_G / 10)
-#define ACCEL_VEL_LAND	VEL_MPS_TO_COUNT(10)
 #define ACCEL_VEL_MACH	VEL_MPS_TO_COUNT(200)
-#define ACCEL_VEL_APOGEE	VEL_MPS_TO_COUNT(2)
-#define ACCEL_VEL_MAIN	VEL_MPS_TO_COUNT(100)
 #define ACCEL_VEL_BOOST	VEL_MPS_TO_COUNT(5)
 
+int32_t accel_vel_mach;
+int32_t accel_vel_boost;
+
 /*
  * Barometer calibration
  *
@@ -170,14 +170,14 @@ ao_flight(void)
 			 * so subtract instead of add.
 			 */
 			ticks = ao_flight_tick - ao_flight_prev_tick;
-			ao_vel_change = (((ao_raw_accel >> 1) + (ao_raw_accel_prev >> 1)) - ao_ground_accel);
+			ao_vel_change = ao_ground_accel - (((ao_raw_accel + 1) >> 1) + ((ao_raw_accel_prev + 1) >> 1));
 			ao_raw_accel_prev = ao_raw_accel;
 
 			/* one is a common interval */
 			if (ticks == 1)
-				ao_flight_vel -= (int32_t) ao_vel_change;
+				ao_flight_vel += (int32_t) ao_vel_change;
 			else
-				ao_flight_vel -= (int32_t) ao_vel_change * (int32_t) ticks;
+				ao_flight_vel += (int32_t) ao_vel_change * (int32_t) ticks;
 
 			ao_flight_adc = ao_adc_ring_next(ao_flight_adc);
 		}
@@ -211,6 +211,9 @@ ao_flight(void)
 			ao_min_pres = ao_ground_pres;
 			ao_config_get();
 			ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy);
+			ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
+			accel_vel_mach = ACCEL_VEL_MACH;
+			accel_vel_boost = ACCEL_VEL_BOOST;
 			ao_flight_vel = 0;
 			ao_min_vel = 0;
 			ao_old_vel = ao_flight_vel;
@@ -218,8 +221,9 @@ ao_flight(void)
 
 			/* Go to pad state if the nose is pointing up */
 			ao_config_get();
-			if (ao_flight_accel < ao_config.accel_zero_g - ACCEL_NOSE_UP) {
-
+			if (ao_config.accel_plus_g != 0 && ao_config.accel_minus_g != 0 &&
+			    ao_flight_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP)
+			{
 				/* Disable the USB controller in flight mode
 				 * to save power
 				 */
@@ -337,19 +341,15 @@ ao_flight(void)
 
 			/* apogee detect: coast to drogue deploy:
 			 *
-			 * accelerometer: abs(velocity) > min_velocity + 2m/s
-			 *               OR
 			 * barometer: fall at least 10m
 			 *
-			 * If the barometer saturates because the flight
-			 * goes over its measuring range (about 53k'),
-			 * requiring a 10m fall will avoid prematurely
-			 * detecting apogee; the accelerometer will take
-			 * over in that case and the integrated velocity
-			 * measurement should suffice to find apogee
+			 * It would be nice to use the accelerometer
+			 * to detect apogee as well, but tests have
+			 * shown that flights far from vertical would
+			 * grossly mis-detect apogee. So, for now,
+			 * we'll trust to a single sensor for this test
 			 */
-			if (/* abs(ao_flight_vel) > ao_min_vel + ACCEL_VEL_APOGEE || */
-			    ao_flight_pres > ao_min_pres + BARO_APOGEE)
+			if (ao_flight_pres > ao_min_pres + BARO_APOGEE)
 			{
 				/* ignite the drogue charge */
 				ao_ignite(ao_igniter_drogue);
@@ -462,7 +462,7 @@ ao_flight_status(void) __reentrant
 {
 	printf("STATE: %7s accel: %d speed: %d altitude: %d main: %d\n",
 	       ao_state_names[ao_flight_state],
-	       AO_ACCEL_COUNT_TO_MSS(ACCEL_ZERO_G - ao_flight_accel),
+	       AO_ACCEL_COUNT_TO_MSS( - ao_flight_accel),
 	       AO_VEL_COUNT_TO_MS(ao_flight_vel),
 	       ao_pres_to_altitude(ao_flight_pres),
 	       ao_pres_to_altitude(ao_main_pres));