projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
Version 1.9.6.2
[fw/altos]
/
src
/
kernel
/
ao_sample.c
diff --git
a/src/kernel/ao_sample.c
b/src/kernel/ao_sample.c
index 91cf113e3a6ae58880be274bec43653154e485fe..2c77c36920ed5755046e158c9e32f4e92e2d9aae 100644
(file)
--- a/
src/kernel/ao_sample.c
+++ b/
src/kernel/ao_sample.c
@@
-57,6
+57,9
@@
angle_t ao_sample_orient;
angle_t ao_sample_orients[AO_NUM_ORIENT];
uint8_t ao_sample_orient_pos;
#endif
angle_t ao_sample_orients[AO_NUM_ORIENT];
uint8_t ao_sample_orient_pos;
#endif
+#ifdef HAS_MOTOR_PRESSURE
+motor_pressure_t ao_sample_motor_pressure;
+#endif
uint8_t ao_sample_data;
uint8_t ao_sample_data;
@@
-87,6
+90,10
@@
int32_t ao_ground_yaw;
int32_t ao_ground_roll;
#endif
int32_t ao_ground_roll;
#endif
+#if HAS_MOTOR_PRESSURE
+motor_pressure_t ao_ground_motor_pressure;
+#endif
+
static uint8_t ao_preflight; /* in preflight mode */
static uint16_t nsamples;
static uint8_t ao_preflight; /* in preflight mode */
static uint16_t nsamples;
@@
-107,6
+114,9
@@
int32_t ao_sample_yaw_sum;
int32_t ao_sample_roll_sum;
static struct ao_quaternion ao_rotation;
#endif
int32_t ao_sample_roll_sum;
static struct ao_quaternion ao_rotation;
#endif
+#if HAS_MOTOR_PRESSURE
+int32_t ao_sample_motor_pressure_sum;
+#endif
#if HAS_FLIGHT_DEBUG
extern uint8_t ao_orient_test;
#if HAS_FLIGHT_DEBUG
extern uint8_t ao_orient_test;
@@
-130,6
+140,9
@@
ao_sample_preflight_add(void)
ao_sample_pitch_sum += ao_sample_pitch;
ao_sample_yaw_sum += ao_sample_yaw;
ao_sample_roll_sum += ao_sample_roll;
ao_sample_pitch_sum += ao_sample_pitch;
ao_sample_yaw_sum += ao_sample_yaw;
ao_sample_roll_sum += ao_sample_roll;
+#endif
+#if HAS_MOTOR_PRESSURE
+ ao_sample_motor_pressure_sum += ao_sample_motor_pressure;
#endif
++nsamples;
}
#endif
++nsamples;
}
@@
-197,15
+210,18
@@
ao_sample_preflight_set(void)
ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
-
+ ao_sample_accel_along_sum = 0;
+ ao_sample_accel_across_sum = 0;
+ ao_sample_accel_through_sum = 0;
+#endif
+#if HAS_MOTOR_PRESSURE
+ ao_ground_motor_pressure = ao_sample_motor_pressure_sum >> 9;
+ ao_sample_motor_pressure_sum = 0;
#endif
#if HAS_GYRO
ao_ground_pitch = ao_sample_pitch_sum;
ao_ground_yaw = ao_sample_yaw_sum;
ao_ground_roll = ao_sample_roll_sum;
#endif
#if HAS_GYRO
ao_ground_pitch = ao_sample_pitch_sum;
ao_ground_yaw = ao_sample_yaw_sum;
ao_ground_roll = ao_sample_roll_sum;
- ao_sample_accel_along_sum = 0;
- ao_sample_accel_across_sum = 0;
- ao_sample_accel_through_sum = 0;
ao_sample_pitch_sum = 0;
ao_sample_yaw_sum = 0;
ao_sample_roll_sum = 0;
ao_sample_pitch_sum = 0;
ao_sample_yaw_sum = 0;
ao_sample_roll_sum = 0;
@@
-241,7
+257,7
@@
ao_sample_preflight_set(void)
(ao_ground_accel_across - ao_config.accel_zero_across),
(ao_ground_accel_through - ao_config.accel_zero_through),
(ao_ground_accel_along - ao_config.accel_zero_along));
(ao_ground_accel_across - ao_config.accel_zero_across),
(ao_ground_accel_through - ao_config.accel_zero_through),
(ao_ground_accel_along - ao_config.accel_zero_along));
-#endif
+#endif
ao_sample_compute_orient();
ao_sample_set_all_orients();
ao_sample_compute_orient();
ao_sample_set_all_orients();
@@
-322,6
+338,10
@@
ao_sample_preflight_update(void)
++nsamples;
else
ao_sample_preflight_set();
++nsamples;
else
ao_sample_preflight_set();
+#if !HAS_BARO
+ if ((nsamples & 0x3f) == 0)
+ ao_kalman_reset_accumulate();
+#endif
}
#if 0
}
#if 0
@@
-375,6
+395,9
@@
ao_sample(void)
ao_sample_yaw = ao_data_yaw(ao_data);
ao_sample_roll = ao_data_roll(ao_data);
#endif
ao_sample_yaw = ao_data_yaw(ao_data);
ao_sample_roll = ao_data_roll(ao_data);
#endif
+#if HAS_MOTOR_PRESSURE
+ ao_sample_motor_pressure = ao_data_motor_pressure(ao_data);
+#endif
if (ao_preflight)
ao_sample_preflight();
if (ao_preflight)
ao_sample_preflight();