X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_sample.h;h=ad7c50eff49081aa4050b9da9f49a87dde4047a2;hp=16d4c5076774d04ad6adefc70064f1dae43d6626;hb=7f46240dfc57164f0c1b0c4c4ed9695bca63860d;hpb=24167015705ae831692b95735968b04a876f935e diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index 16d4c507..ad7c50ef 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -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 - * 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 @@ -24,6 +25,24 @@ * 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 * @@ -76,7 +95,12 @@ /* * 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) @@ -87,70 +111,100 @@ * 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_ */