projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos
[fw/altos]
/
src
/
kernel
/
ao_flight.c
diff --git
a/src/kernel/ao_flight.c
b/src/kernel/ao_flight.c
index 2142546c9b0737b186cca6f6dce65fa51c8e30ec..cee8b2b8b38d008c16460b5c293b8c4b326c005d 100644
(file)
--- a/
src/kernel/ao_flight.c
+++ b/
src/kernel/ao_flight.c
@@
-50,8
+50,8
@@
/* Main flight thread. */
enum ao_flight_state ao_flight_state; /* current flight state */
/* Main flight thread. */
enum ao_flight_state ao_flight_state; /* current flight state */
-
uint16_t
ao_boost_tick; /* time of most recent boost detect */
-
uint16_t
ao_launch_tick; /* time of first boost detect */
+
AO_TICK_TYPE
ao_boost_tick; /* time of most recent boost detect */
+
AO_TICK_TYPE
ao_launch_tick; /* time of first boost detect */
uint16_t ao_motor_number; /* number of motors burned so far */
#if HAS_SENSOR_ERRORS
uint16_t ao_motor_number; /* number of motors burned so far */
#if HAS_SENSOR_ERRORS
@@
-63,7
+63,7
@@
uint8_t ao_sensor_errors;
* track min/max data over a long interval to detect
* resting
*/
* track min/max data over a long interval to detect
* resting
*/
-static
uint16_t
ao_interval_end;
+static
AO_TICK_TYPE
ao_interval_end;
#ifdef HAS_BARO
static ao_v_t ao_interval_min_height;
static ao_v_t ao_interval_max_height;
#ifdef HAS_BARO
static ao_v_t ao_interval_min_height;
static ao_v_t ao_interval_max_height;
@@
-102,6
+102,10
@@
uint8_t ao_flight_force_idle;
#define abs(a) ((a) < 0 ? -(a) : (a))
#define abs(a) ((a) < 0 ? -(a) : (a))
+#if !HAS_BARO
+// #define DEBUG_ACCEL_ONLY 1
+#endif
+
void
ao_flight(void)
{
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 ||
#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
#if HAS_BARO
|| ao_ground_height < -1000 ||
ao_ground_height > 7000
@@
-200,6
+204,15
@@
ao_flight(void)
ao_wakeup(&ao_flight_state);
break;
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:
*
case ao_flight_pad:
/* pad to boost:
*
@@
-216,8
+229,8
@@
ao_flight(void)
*/
if (ao_height > AO_M_TO_HEIGHT(20)
#if HAS_ACCEL
*/
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
)
{
#endif
)
{
@@
-259,7
+272,7
@@
ao_flight(void)
* (15 seconds) has past.
*/
if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) ||
* (15 seconds) has past.
*/
if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) ||
- (
int16_t) (ao_sample_tick - ao_boost_tick) >
BOOST_TICKS_MAX)
+ (
AO_TICK_SIGNED) (ao_sample_tick - ao_boost_tick) > (AO_TICK_SIGNED)
BOOST_TICKS_MAX)
{
#if HAS_ACCEL
#if HAS_BARO
{
#if HAS_ACCEL
#if HAS_BARO
@@
-306,8
+319,8
@@
ao_flight(void)
* number of seconds.
*/
if (ao_config.apogee_lockout) {
* number of seconds.
*/
if (ao_config.apogee_lockout) {
- if ((
int16_t
) (ao_sample_tick - ao_launch_tick) <
- AO_SEC_TO_TICKS(ao_config.apogee_lockout))
+ if ((
AO_TICK_SIGNED
) (ao_sample_tick - ao_launch_tick) <
+
(AO_TICK_SIGNED)
AO_SEC_TO_TICKS(ao_config.apogee_lockout))
break;
}
break;
}
@@
-350,7
+363,7
@@
ao_flight(void)
#define MAX_QUIET_ACCEL 2
#define MAX_QUIET_ACCEL 2
- if ((
int16_t
) (ao_sample_tick - ao_interval_end) >= 0) {
+ if ((
AO_TICK_SIGNED
) (ao_sample_tick - ao_interval_end) >= 0) {
if (ao_interval_max_accel_along - ao_interval_min_accel_along <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) &&
ao_interval_max_accel_across - ao_interval_min_accel_across <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) &&
ao_interval_max_accel_through - ao_interval_min_accel_through <= ao_data_accel_to_sample(MAX_QUIET_ACCEL))
if (ao_interval_max_accel_along - ao_interval_min_accel_along <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) &&
ao_interval_max_accel_across - ao_interval_min_accel_across <= ao_data_accel_to_sample(MAX_QUIET_ACCEL) &&
ao_interval_max_accel_through - ao_interval_min_accel_through <= ao_data_accel_to_sample(MAX_QUIET_ACCEL))
@@
-430,7
+443,7
@@
ao_flight(void)
if (ao_avg_height > ao_interval_max_height)
ao_interval_max_height = ao_avg_height;
if (ao_avg_height > ao_interval_max_height)
ao_interval_max_height = ao_avg_height;
- if ((
int16_t
) (ao_sample_tick - ao_interval_end) >= 0) {
+ if ((
AO_TICK_SIGNED
) (ao_sample_tick - ao_interval_end) >= 0) {
if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4))
{
ao_flight_state = ao_flight_landed;
if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4))
{
ao_flight_state = ao_flight_landed;
@@
-448,7
+461,7
@@
ao_flight(void)
#if HAS_FLIGHT_DEBUG
case ao_flight_test:
#if HAS_GYRO
#if HAS_FLIGHT_DEBUG
case ao_flight_test:
#if HAS_GYRO
- printf ("angle %4d pitch %7
d yaw %7d roll %7
d\n",
+ printf ("angle %4d pitch %7
ld yaw %7ld roll %7l
d\n",
ao_sample_orient,
((ao_sample_pitch << 9) - ao_ground_pitch) >> 9,
((ao_sample_yaw << 9) - ao_ground_yaw) >> 9,
ao_sample_orient,
((ao_sample_pitch << 9) - ao_ground_pitch) >> 9,
((ao_sample_yaw << 9) - ao_ground_yaw) >> 9,
@@
-479,14
+492,14
@@
ao_flight_dump(void)
printf ("sample:\n");
printf (" tick %d\n", ao_sample_tick);
#if HAS_BARO
printf ("sample:\n");
printf (" tick %d\n", ao_sample_tick);
#if HAS_BARO
- printf (" raw pres %d\n", ao_sample_pres);
+ printf (" raw pres %
l
d\n", ao_sample_pres);
#endif
#if HAS_ACCEL
printf (" raw accel %d\n", ao_sample_accel);
#endif
#if HAS_BARO
#endif
#if HAS_ACCEL
printf (" raw accel %d\n", ao_sample_accel);
#endif
#if HAS_BARO
- printf (" ground pres %d\n", ao_ground_pres);
- printf (" ground alt %d\n", ao_ground_height);
+ printf (" ground pres %
l
d\n", ao_ground_pres);
+ printf (" ground alt %
l
d\n", ao_ground_height);
#endif
#if HAS_ACCEL
printf (" raw accel %d\n", ao_sample_accel);
#endif
#if HAS_ACCEL
printf (" raw accel %d\n", ao_sample_accel);
@@
-495,8
+508,8
@@
ao_flight_dump(void)
#endif
#if HAS_BARO
#endif
#if HAS_BARO
- printf (" alt %d\n", ao_sample_alt);
- printf (" height %d\n", ao_sample_height);
+ printf (" alt %
l
d\n", ao_sample_alt);
+ printf (" height %
l
d\n", ao_sample_height);
#endif
#if HAS_ACCEL
#endif
#if HAS_ACCEL
@@
-505,12
+518,12
@@
ao_flight_dump(void)
printf ("kalman:\n");
printf ("kalman:\n");
- printf (" height %d\n", ao_height);
+ printf (" height %
l
d\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 (" 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 %
l
d\n", ao_max_height);
+ printf (" avg_height %
l
d\n", ao_avg_height);
+ printf (" error_h %
l
d\n", ao_error_h);
#if !HAS_ACCEL
printf (" error_avg %d\n", ao_error_h_sq_avg);
#endif
#if !HAS_ACCEL
printf (" error_avg %d\n", ao_error_h_sq_avg);
#endif
@@
-530,6
+543,7
@@
static void
ao_orient_test_select(void)
{
ao_orient_test = !ao_orient_test;
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[] = {
}
const struct ao_cmds ao_flight_cmds[] = {