projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
altos: Remove *_TO_DATA macros
[fw/altos]
/
src
/
kernel
/
ao_flight.c
diff --git
a/src/kernel/ao_flight.c
b/src/kernel/ao_flight.c
index 24099347ce4466589592c4d49ffc78775de867fe..170396ff160ab41473f656d8b125d9b8f593b623 100644
(file)
--- a/
src/kernel/ao_flight.c
+++ b/
src/kernel/ao_flight.c
@@
-3,7
+3,8
@@
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
@@
-20,7
+21,7
@@
#include <ao_log.h>
#endif
#include <ao_log.h>
#endif
-#if HAS_MPU6000
+#if HAS_MPU6000
|| HAS_MPU9250
#include <ao_quaternion.h>
#endif
#include <ao_quaternion.h>
#endif
@@
-36,6
+37,10
@@
#error Please define HAS_USB
#endif
#error Please define HAS_USB
#endif
+#if HAS_FAKE_FLIGHT
+#include <ao_fake_flight.h>
+#endif
+
#ifndef HAS_TELEMETRY
#define HAS_TELEMETRY HAS_RADIO
#endif
#ifndef HAS_TELEMETRY
#define HAS_TELEMETRY HAS_RADIO
#endif
@@
-43,7
+48,8
@@
/* Main flight thread. */
__pdata enum ao_flight_state ao_flight_state; /* current flight state */
/* Main flight thread. */
__pdata enum ao_flight_state ao_flight_state; /* current flight state */
-__pdata uint16_t ao_boost_tick; /* time of launch detect */
+__pdata uint16_t ao_boost_tick; /* time of most recent boost detect */
+__pdata uint16_t ao_launch_tick; /* time of first boost detect */
__pdata uint16_t ao_motor_number; /* number of motors burned so far */
#if HAS_SENSOR_ERRORS
__pdata uint16_t ao_motor_number; /* number of motors burned so far */
#if HAS_SENSOR_ERRORS
@@
-56,10
+62,10
@@
__xdata uint8_t ao_sensor_errors;
* resting
*/
static __data uint16_t ao_interval_end;
* resting
*/
static __data uint16_t ao_interval_end;
-static __data
int16_t
ao_interval_min_height;
-static __data
int16_t
ao_interval_max_height;
+static __data
ao_v_t
ao_interval_min_height;
+static __data
ao_v_t
ao_interval_max_height;
#if HAS_ACCEL
#if HAS_ACCEL
-static __data
int16_t
ao_coast_avg_accel;
+static __data
ao_v_t
ao_coast_avg_accel;
#endif
__pdata uint8_t ao_flight_force_idle;
#endif
__pdata uint8_t ao_flight_force_idle;
@@
-126,11
+132,14
@@
ao_flight(void)
{
/* Set pad mode - we can fly! */
ao_flight_state = ao_flight_pad;
{
/* Set pad mode - we can fly! */
ao_flight_state = ao_flight_pad;
-#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE
+#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE
&& !DEBUG
/* Disable the USB controller in flight mode
* to save power
*/
/* Disable the USB controller in flight mode
* to save power
*/
- ao_usb_disable();
+#if HAS_FAKE_FLIGHT
+ if (!ao_fake_flight_active)
+#endif
+ ao_usb_disable();
#endif
#if !HAS_ACCEL && PACKET_HAS_SLAVE
#endif
#if !HAS_ACCEL && PACKET_HAS_SLAVE
@@
-166,11
+175,10
@@
ao_flight(void)
#endif
}
/* wakeup threads due to state change */
#endif
}
/* wakeup threads due to state change */
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
break;
case ao_flight_pad:
break;
case ao_flight_pad:
-
/* pad to boost:
*
* barometer: > 20m vertical motion
/* pad to boost:
*
* barometer: > 20m vertical motion
@@
-192,7
+200,7
@@
ao_flight(void)
)
{
ao_flight_state = ao_flight_boost;
)
{
ao_flight_state = ao_flight_boost;
- ao_boost_tick = ao_sample_tick;
+ ao_
launch_tick = ao_
boost_tick = ao_sample_tick;
/* start logging data */
ao_log_start();
/* start logging data */
ao_log_start();
@@
-211,7
+219,7
@@
ao_flight(void)
ao_wakeup(&ao_gps_new);
#endif
ao_wakeup(&ao_gps_new);
#endif
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
}
break;
case ao_flight_boost:
}
break;
case ao_flight_boost:
@@
-226,7
+234,7
@@
ao_flight(void)
* deceleration, or by waiting until the maximum burn duration
* (15 seconds) has past.
*/
* deceleration, or by waiting until the maximum burn duration
* (15 seconds) has past.
*/
- if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)
&& ao_height > AO_M_TO_HEIGHT(100)
) ||
+ if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) ||
(int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX)
{
#if HAS_ACCEL
(int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX)
{
#if HAS_ACCEL
@@
-236,7
+244,7
@@
ao_flight(void)
ao_flight_state = ao_flight_coast;
#endif
++ao_motor_number;
ao_flight_state = ao_flight_coast;
#endif
++ao_motor_number;
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
}
break;
#if HAS_ACCEL
}
break;
#if HAS_ACCEL
@@
-249,7
+257,7
@@
ao_flight(void)
if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED))
{
ao_flight_state = ao_flight_coast;
if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED))
{
ao_flight_state = ao_flight_coast;
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
} else
goto check_re_boost;
break;
} else
goto check_re_boost;
break;
@@
-262,7
+270,7
@@
ao_flight(void)
* number of seconds.
*/
if (ao_config.apogee_lockout) {
* number of seconds.
*/
if (ao_config.apogee_lockout) {
- if ((
ao_sample_tick - ao_boost
_tick) <
+ if ((
int16_t) (ao_sample_tick - ao_launch
_tick) <
AO_SEC_TO_TICKS(ao_config.apogee_lockout))
break;
}
AO_SEC_TO_TICKS(ao_config.apogee_lockout))
break;
}
@@
-275,9
+283,11
@@
ao_flight(void)
* the measured altitude reasonably closely; otherwise
* we're probably transsonic.
*/
* the measured altitude reasonably closely; otherwise
* we're probably transsonic.
*/
+#define AO_ERROR_BOUND 100
+
if (ao_speed < 0
#if !HAS_ACCEL
if (ao_speed < 0
#if !HAS_ACCEL
- && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg <
100
)
+ && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg <
AO_ERROR_BOUND
)
#endif
)
{
#endif
)
{
@@
-296,16
+306,16
@@
ao_flight(void)
/* and enter drogue state */
ao_flight_state = ao_flight_drogue;
/* and enter drogue state */
ao_flight_state = ao_flight_drogue;
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
}
#if HAS_ACCEL
else {
check_re_boost:
}
#if HAS_ACCEL
else {
check_re_boost:
- ao_coast_avg_accel = ao_coast_avg_accel
- (ao_coast_avg_accel >> 6) + (ao_accel >> 6
);
+ ao_coast_avg_accel = ao_coast_avg_accel
+ ((ao_accel - ao_coast_avg_accel) >> 5
);
if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) {
ao_boost_tick = ao_sample_tick;
ao_flight_state = ao_flight_boost;
if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) {
ao_boost_tick = ao_sample_tick;
ao_flight_state = ao_flight_boost;
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
}
}
#endif
}
}
#endif
@@
-342,7
+352,7
@@
ao_flight(void)
ao_interval_min_height = ao_interval_max_height = ao_avg_height;
ao_flight_state = ao_flight_main;
ao_interval_min_height = ao_interval_max_height = ao_avg_height;
ao_flight_state = ao_flight_main;
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ ao_wakeup(
&ao_flight_state
);
}
break;
}
break;
@@
-364,10
+374,12
@@
ao_flight(void)
{
ao_flight_state = ao_flight_landed;
{
ao_flight_state = ao_flight_landed;
+#if HAS_ADC
/* turn off the ADC capture */
ao_timer_set_adc_interval(0);
/* turn off the ADC capture */
ao_timer_set_adc_interval(0);
+#endif
- ao_wakeup(
DATA_TO_XDATA(&ao_flight_state)
);
+ 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;
}
ao_interval_min_height = ao_interval_max_height = ao_avg_height;
ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
@@
-392,14
+404,14
@@
ao_flight(void)
}
#if HAS_FLIGHT_DEBUG
}
#if HAS_FLIGHT_DEBUG
-static inline int int_part(
int16_t i)
{ return i >> 4; }
-static inline int frac_part(
int16_t i)
{ return ((i & 0xf) * 100 + 8) / 16; }
+static inline int int_part(
ao_v_t i)
{ return i >> 4; }
+static inline int frac_part(
ao_v_t i)
{ return ((i & 0xf) * 100 + 8) / 16; }
static void
ao_flight_dump(void)
{
#if HAS_ACCEL
static void
ao_flight_dump(void)
{
#if HAS_ACCEL
-
int16_t
accel;
+
ao_v_t
accel;
accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
#endif
accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
#endif