projects / fw / altos / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
altos: Fix up HAS_FLIGHT_DEBUG
[fw/altos] / src / kernel / ao_flight.c
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index afee1de710935b00c6a75e3b3f2498beb23be6a8..4a7055c415e80d9eef3d86eacae715ce4f8b416b 100644 (file)
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -102,6 +102,10 @@ uint8_t                    ao_flight_force_idle;
 
 #define abs(a) ((a) < 0 ? -(a) : (a))
 
+#if !HAS_BARO
+// #define DEBUG_ACCEL_ONLY    1
+#endif
+
 void
 ao_flight(void)
 {
@@ -127,8 +131,8 @@ ao_flight(void)
 #if HAS_ACCEL
                        if (ao_config.accel_plus_g == 0 ||
                            ao_config.accel_minus_g == 0 ||
-                           ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
-                           ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP
+                           ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP ||
+                           ao_ground_accel > (accel_t) ao_config.accel_minus_g + ACCEL_NOSE_UP
 #if HAS_BARO
                            || ao_ground_height < -1000 ||
                            ao_ground_height > 7000
@@ -200,6 +204,15 @@ ao_flight(void)
                        ao_wakeup(&ao_flight_state);
 
                        break;
+
+#if DEBUG_ACCEL_ONLY
+               case ao_flight_invalid:
+               case ao_flight_idle:
+                       printf("+g %d ga %d sa %d accel %ld speed %ld\n",
+                              ao_config.accel_plus_g, ao_ground_accel, ao_sample_accel, ao_accel, ao_speed);
+                       break;
+#endif
+
                case ao_flight_pad:
                        /* pad to boost:
                         *
@@ -216,12 +229,14 @@ ao_flight(void)
                         */
                        if (ao_height > AO_M_TO_HEIGHT(20)
 #if HAS_ACCEL
-                           || (ao_accel > AO_MSS_TO_ACCEL(20) &&
-                               ao_speed > AO_MS_TO_SPEED(5))
+                           || (ao_accel > AO_MSS_TO_ACCEL(20)
+                               && ao_speed > AO_MS_TO_SPEED(5))
 #endif
                                )
                        {
                                ao_flight_state = ao_flight_boost;
+                               ao_wakeup(&ao_flight_state);
+
                                ao_launch_tick = ao_boost_tick = ao_sample_tick;
 
                                /* start logging data */
@@ -242,8 +257,6 @@ ao_flight(void)
                                ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
                                ao_wakeup(&ao_gps_new);
 #endif
-
-                               ao_wakeup(&ao_flight_state);
                        }
                        break;
                case ao_flight_boost:
@@ -278,8 +291,8 @@ ao_flight(void)
 #else
                                ao_flight_state = ao_flight_coast;
 #endif
-                               ++ao_motor_number;
                                ao_wakeup(&ao_flight_state);
+                               ++ao_motor_number;
                        }
                        break;
 #if HAS_ACCEL && HAS_BARO
@@ -327,11 +340,9 @@ ao_flight(void)
 #endif
                                )
                        {
-#if HAS_IGNITE
-                               /* ignite the drogue charge */
-                               ao_ignite(ao_igniter_drogue);
-#endif
-
+                               /* enter drogue state */
+                               ao_flight_state = ao_flight_drogue;
+                               ao_wakeup(&ao_flight_state);
 #if HAS_TELEMETRY
                                /* slow down the telemetry system */
                                ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER);
@@ -339,10 +350,6 @@ ao_flight(void)
                                /* Turn the RDF beacon back on */
                                ao_rdf_set(1);
 #endif
-
-                               /* and enter drogue state */
-                               ao_flight_state = ao_flight_drogue;
-                               ao_wakeup(&ao_flight_state);
                        }
                        else
 #else /* not HAS_BARO */
@@ -362,12 +369,11 @@ ao_flight(void)
                                    ao_interval_max_accel_through - ao_interval_min_accel_through <= ao_data_accel_to_sample(MAX_QUIET_ACCEL))
                                {
                                        ao_flight_state = ao_flight_landed;
+                                       ao_wakeup(&ao_flight_state);
 #if HAS_ADC
                                        /* turn off the ADC capture */
                                        ao_timer_set_adc_interval(0);
 #endif
-
-                                       ao_wakeup(&ao_flight_state);
                                }
 
                                /* Reset interval values */
@@ -410,9 +416,8 @@ ao_flight(void)
                         */
                        if (ao_height <= ao_config.main_deploy)
                        {
-#if HAS_IGNITE
-                               ao_ignite(ao_igniter_main);
-#endif
+                               ao_flight_state = ao_flight_main;
+                               ao_wakeup(&ao_flight_state);
 
                                /*
                                 * Start recording min/max height
@@ -423,9 +428,6 @@ ao_flight(void)
                                ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
 
                                ao_interval_min_height = ao_interval_max_height = ao_avg_height;
-
-                               ao_flight_state = ao_flight_main;
-                               ao_wakeup(&ao_flight_state);
                        }
                        break;
 
@@ -445,13 +447,11 @@ ao_flight(void)
                                if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4))
                                {
                                        ao_flight_state = ao_flight_landed;
-
+                                       ao_wakeup(&ao_flight_state);
 #if HAS_ADC
                                        /* turn off the ADC capture */
                                        ao_timer_set_adc_interval(0);
 #endif
-
-                                       ao_wakeup(&ao_flight_state);
                                }
                                ao_interval_min_height = ao_interval_max_height = ao_avg_height;
                                ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
@@ -461,7 +461,7 @@ ao_flight(void)
 #if HAS_FLIGHT_DEBUG
                case ao_flight_test:
 #if HAS_GYRO
-                       printf ("angle %4d pitch %7d yaw %7d roll %7d\n",
+                       printf ("angle %4d pitch %7ld yaw %7ld roll %7ld\n",
                                ao_sample_orient,
                                ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9,
                                ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9,
@@ -491,32 +491,39 @@ ao_flight_dump(void)
 
        printf ("sample:\n");
        printf ("  tick        %d\n", ao_sample_tick);
-       printf ("  raw pres    %d\n", ao_sample_pres);
+#if HAS_BARO
+       printf ("  raw pres    %ld\n", ao_sample_pres);
+#endif
 #if HAS_ACCEL
        printf ("  raw accel   %d\n", ao_sample_accel);
 #endif
-       printf ("  ground pres %d\n", ao_ground_pres);
-       printf ("  ground alt  %d\n", ao_ground_height);
+#if HAS_BARO
+       printf ("  ground pres %ld\n", ao_ground_pres);
+       printf ("  ground alt  %ld\n", ao_ground_height);
+#endif
 #if HAS_ACCEL
        printf ("  raw accel   %d\n", ao_sample_accel);
        printf ("  groundaccel %d\n", ao_ground_accel);
        printf ("  accel_2g    %d\n", ao_accel_2g);
 #endif
 
-       printf ("  alt         %d\n", ao_sample_alt);
-       printf ("  height      %d\n", ao_sample_height);
+#if HAS_BARO
+       printf ("  alt         %ld\n", ao_sample_alt);
+       printf ("  height      %ld\n", ao_sample_height);
+#endif
+
 #if HAS_ACCEL
        printf ("  accel       %d.%02d\n", int_part(accel), frac_part(accel));
 #endif
 
 
        printf ("kalman:\n");
-       printf ("  height      %d\n", ao_height);
+       printf ("  height      %ld\n", ao_height);
        printf ("  speed       %d.%02d\n", int_part(ao_speed), frac_part(ao_speed));
        printf ("  accel       %d.%02d\n", int_part(ao_accel), frac_part(ao_accel));
-       printf ("  max_height  %d\n", ao_max_height);
-       printf ("  avg_height  %d\n", ao_avg_height);
-       printf ("  error_h     %d\n", ao_error_h);
+       printf ("  max_height  %ld\n", ao_max_height);
+       printf ("  avg_height  %ld\n", ao_avg_height);
+       printf ("  error_h     %ld\n", ao_error_h);
 #if !HAS_ACCEL
        printf ("  error_avg   %d\n", ao_error_h_sq_avg);
 #endif
@@ -536,6 +543,7 @@ static void
 ao_orient_test_select(void)
 {
        ao_orient_test = !ao_orient_test;
+       printf("orient test %d\n", ao_orient_test);
 }
 
 const struct ao_cmds ao_flight_cmds[] = {
AltOS - the operating system for Altus Metrum products