*
* 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
* ao_sample.c
*/
+#ifndef AO_VALUE_32
+#define AO_VALUE_32 1
+#endif
+
+#if AO_VALUE_32
+/*
+ * For 32-bit computed values, use 64-bit intermediates.
+ */
+typedef int64_t ao_k_t;
+typedef int32_t ao_v_t;
+#else
+/*
+ * For 16-bit computed values, use 32-bit intermediates.
+ */
+typedef int32_t ao_k_t;
+typedef int16_t ao_v_t;
+#endif
+
/*
* Barometer calibration
*
/*
* Above this speed, baro measurements are unreliable
*/
-#define AO_MAX_BARO_SPEED 200
+#define AO_MAX_BARO_SPEED 248
+
+/* The maximum amount (in a range of 0-256) to de-rate the
+ * baro sensor data based on speed.
+ */
+#define AO_MAX_SPEED_DISTRUST 160
#define ACCEL_NOSE_UP (ao_accel_2g >> 2)
* 2047m/s² (over 200g)
*/
-#define AO_M_TO_HEIGHT(m) ((int16_t) (m))
-#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16))
-#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16))
+#define AO_M_TO_HEIGHT(m) ((ao_v_t) (m))
+#define AO_MS_TO_SPEED(ms) ((ao_v_t) ((ms) * 16))
+#define AO_MSS_TO_ACCEL(mss) ((ao_v_t) ((mss) * 16))
-extern __pdata uint16_t ao_sample_tick; /* time of last data */
-extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */
-extern __data uint8_t ao_sample_data; /* Ring position of last processed sample */
+extern AO_TICK_TYPE ao_sample_tick; /* time of last data */
+extern uint8_t ao_sample_adc; /* Ring position of last processed sample */
+extern uint8_t ao_sample_data; /* Ring position of last processed sample */
#if HAS_BARO
-extern __pdata pres_t ao_sample_pres; /* most recent pressure sensor reading */
-extern __pdata alt_t ao_sample_alt; /* MSL of ao_sample_pres */
-extern __pdata alt_t ao_sample_height; /* AGL of ao_sample_pres */
-extern __pdata pres_t ao_ground_pres; /* startup pressure */
-extern __pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */
+extern pres_t ao_sample_pres; /* most recent pressure sensor reading */
+extern alt_t ao_sample_alt; /* MSL of ao_sample_pres */
+extern alt_t ao_sample_height; /* AGL of ao_sample_pres */
+extern pres_t ao_ground_pres; /* startup pressure */
+extern alt_t ao_ground_height; /* MSL of ao_ground_pres */
#endif
#if HAS_ACCEL
-extern __pdata accel_t ao_sample_accel; /* most recent accel sensor reading */
-extern __pdata accel_t ao_ground_accel; /* startup acceleration */
-extern __pdata accel_t ao_accel_2g; /* factory accel calibration */
-extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */
+extern accel_t ao_sample_accel; /* most recent accel sensor reading */
+extern accel_t ao_ground_accel; /* startup acceleration */
+extern accel_t ao_accel_2g; /* factory accel calibration */
+extern int32_t ao_accel_scale; /* sensor to m/s² conversion */
+#endif
+#if HAS_IMU
+extern accel_t ao_ground_accel_along;
+extern accel_t ao_ground_accel_across;
+extern accel_t ao_ground_accel_through;
+extern accel_t ao_sample_accel_along;
+extern accel_t ao_sample_accel_across;
+extern accel_t ao_sample_accel_through;
#endif
#if HAS_GYRO
-extern __pdata accel_t ao_ground_accel_along;
-extern __pdata accel_t ao_ground_accel_across;
-extern __pdata accel_t ao_ground_accel_through;
-extern __pdata int32_t ao_ground_pitch; /* * 512 */
-extern __pdata int32_t ao_ground_yaw; /* * 512 */
-extern __pdata int32_t ao_ground_roll; /* * 512 */
-extern __pdata accel_t ao_sample_accel_along;
-extern __pdata accel_t ao_sample_accel_across;
-extern __pdata accel_t ao_sample_accel_through;
-extern __pdata gyro_t ao_sample_roll;
-extern __pdata gyro_t ao_sample_pitch;
-extern __pdata gyro_t ao_sample_yaw;
-extern __pdata angle_t ao_sample_orient;
+#ifndef HAS_IMU
+#define HAS_IMU 1
+#endif
+extern int32_t ao_ground_pitch; /* * 512 */
+extern int32_t ao_ground_yaw; /* * 512 */
+extern int32_t ao_ground_roll; /* * 512 */
+extern gyro_t ao_sample_roll;
+extern gyro_t ao_sample_pitch;
+extern gyro_t ao_sample_yaw;
+#define AO_NUM_ORIENT 64
+extern angle_t ao_sample_orient;
+extern angle_t ao_sample_orients[AO_NUM_ORIENT];
+extern uint8_t ao_sample_orient_pos;
+#endif
+#if HAS_MOTOR_PRESSURE
+extern motor_pressure_t ao_ground_motor_pressure;
+extern motor_pressure_t ao_sample_motor_pressure;
#endif
void ao_sample_init(void);
-/* returns FALSE in preflight mode, TRUE in flight mode */
+/* returns false in preflight mode, true in flight mode */
uint8_t ao_sample(void);
/*
* ao_kalman.c
*/
-#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
-#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5))
+
+#ifdef AO_VALUE_32
+#if AO_VALUE_32
+#define to_fix_v(x) to_fix_32(x)
+#define to_fix_k(x) to_fix_64(x)
+#else
+#define to_fix_v(x) to_fix_16(x)
+#define to_fix_k(x) to_fix_32(x)
+#endif
+
#define from_fix(x) ((x) >> 16)
-extern __pdata int16_t ao_height; /* meters */
-extern __pdata int16_t ao_speed; /* m/s * 16 */
-extern __pdata int16_t ao_accel; /* m/s² * 16 */
-extern __xdata int16_t ao_max_height; /* max of ao_height */
-extern __xdata int16_t ao_avg_height; /* running average of height */
+extern ao_v_t ao_height; /* meters */
+extern ao_v_t ao_speed; /* m/s * 16 */
+extern ao_v_t ao_accel; /* m/s² * 16 */
+extern ao_v_t ao_max_height; /* max of ao_height */
+extern ao_v_t ao_avg_height; /* running average of height */
-extern __pdata int16_t ao_error_h;
-extern __pdata int16_t ao_error_h_sq_avg;
+extern ao_v_t ao_error_h;
+#if !HAS_ACCEL
+extern ao_v_t ao_error_h_sq_avg;
+#endif
#if HAS_ACCEL
-extern __pdata int16_t ao_error_a;
+extern ao_v_t ao_error_a;
+#endif
#endif
void ao_kalman(void);
+#if !HAS_BARO
+void ao_kalman_reset_accumulate(void);
+#endif
+
#endif /* _AO_SAMPLE_H_ */
projects / fw / altos / blobdiff