diff options
| -rw-r--r-- | src/core/ao_flight.c | 21 | ||||
| -rw-r--r-- | src/core/ao_flight.h | 3 |
2 files changed, 23 insertions, 1 deletions
diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 240b348a..4a53bdc6 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -364,6 +364,18 @@ ao_flight(void) ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; } break; +#if HAS_FLIGHT_DEBUG + case ao_flight_test: +#if HAS_GYRO + printf ("angle %4d pitch %7d yaw %7d roll %7d\n", + ao_sample_orient, + ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, + ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, + ((ao_sample_roll << 9) - ao_ground_roll) >> 9); +#endif + flush(); + break; +#endif /* HAS_FLIGHT_DEBUG */ default: break; } @@ -414,8 +426,17 @@ ao_flight_dump(void) printf (" error_avg %d\n", ao_error_h_sq_avg); } +static void +ao_gyro_test(void) +{ + ao_flight_state = ao_flight_test; + ao_getchar(); + ao_flight_state = ao_flight_idle; +} + __code struct ao_cmds ao_flight_cmds[] = { { ao_flight_dump, "F\0Dump flight status" }, + { ao_gyro_test, "G\0Test gyro code" }, { 0, NULL }, }; #endif diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index b80202f0..ac3e9cfb 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -33,7 +33,8 @@ enum ao_flight_state { ao_flight_drogue = 6, ao_flight_main = 7, ao_flight_landed = 8, - ao_flight_invalid = 9 + ao_flight_invalid = 9, + ao_flight_test = 10 }; extern __pdata enum ao_flight_state ao_flight_state; |