Make menu seperator insensitive

[fw/altos] / ao_flight.c

diff --git a/ao_flight.c b/ao_flight.c
index 48f7120260ef981b5c58c66fae449ab58334891d..c0f5683047b4d5e3e37dd6d28d14bb9de71295e2 100644 (file)
--- a/ao_flight.c
+++ b/ao_flight.c
@@ -125,6 +125,8 @@ __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres;
  */
 __pdata int32_t        ao_flight_vel;
 __pdata int32_t ao_min_vel;
+__pdata int32_t        ao_old_vel;
+__pdata int16_t ao_old_vel_tick;
 __xdata int32_t ao_raw_accel_sum, ao_raw_pres_sum;
 
 /* Landing is detected by getting constant readings from both pressure and accelerometer
@@ -168,7 +170,7 @@ ao_flight(void)
                         * so subtract instead of add.
                         */
                        ticks = ao_flight_tick - ao_flight_prev_tick;
-                       ao_vel_change = (((ao_raw_accel + ao_raw_accel_prev) >> 1) - ao_ground_accel);
+                       ao_vel_change = (((ao_raw_accel >> 1) + (ao_raw_accel_prev >> 1)) - ao_ground_accel);
                        ao_raw_accel_prev = ao_raw_accel;
 
                        /* one is a common interval */
@@ -211,6 +213,8 @@ ao_flight(void)
                        ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy);
                        ao_flight_vel = 0;
                        ao_min_vel = 0;
+                       ao_old_vel = ao_flight_vel;
+                       ao_old_vel_tick = ao_flight_tick;
 
                        /* Go to launchpad state if the nose is pointing up */
                        ao_config_get();
@@ -223,7 +227,6 @@ ao_flight(void)
 
                                /* Turn on telemetry system
                                 */
-                               ao_rdf_set(1);
                                ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
 
                                ao_flight_state = ao_flight_launchpad;
@@ -241,6 +244,16 @@ ao_flight(void)
                        break;
                case ao_flight_launchpad:
 
+                       /* Trim velocity
+                        *
+                        * Once a second, remove any velocity from
+                        * a second ago
+                        */
+                       if ((int16_t) (ao_flight_tick - ao_old_vel_tick) >= AO_SEC_TO_TICKS(1)) {
+                               ao_old_vel_tick = ao_flight_tick;
+                               ao_flight_vel -= ao_old_vel;
+                               ao_old_vel = ao_flight_vel;
+                       }
                        /* pad to boost:
                         *
                         * accelerometer: > 2g AND velocity > 5m/s
@@ -264,9 +277,6 @@ ao_flight(void)
                                /* Increase telemetry rate */
                                ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT);
 
-                               /* disable RDF beacon */
-                               ao_rdf_set(0);
-
                                ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
                                break;
                        }
@@ -346,9 +356,6 @@ ao_flight(void)
                                /* slow down the ADC sample rate */
                                ao_timer_set_adc_interval(10);
 
-                               /* Enable RDF beacon */
-                               ao_rdf_set(1);
-
                                /*
                                 * Start recording min/max accel and pres for a while
                                 * to figure out when the rocket has landed