X-Git-Url: https://git.gag.com/?a=blobdiff_plain;ds=sidebyside;f=src%2Fkernel%2Fao_sample.c;h=3f5fc7a98c557af012d053dd0b14068305f6924b;hb=HEAD;hp=9cba36c1004b37ec31717e7c4431e08ede4bf615;hpb=0686a7b8aec524d81bda4c572549a3a068ce0eed;p=fw%2Faltos diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 9cba36c1..422ccec6 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -21,6 +21,10 @@ #include #endif +#ifndef HAS_KALMAN +#define HAS_KALMAN 1 +#endif + #if HAS_GYRO #include #endif @@ -35,17 +39,21 @@ #define ACCEL_TYPE int16_t #endif -uint16_t ao_sample_tick; /* time of last data */ +AO_TICK_TYPE ao_sample_tick; /* time of last data */ +#if HAS_BARO pres_t ao_sample_pres; alt_t ao_sample_alt; alt_t ao_sample_height; +#endif #if HAS_ACCEL accel_t ao_sample_accel; #endif -#if HAS_GYRO +#if HAS_IMU accel_t ao_sample_accel_along; accel_t ao_sample_accel_across; accel_t ao_sample_accel_through; +#endif +#if HAS_GYRO gyro_t ao_sample_roll; gyro_t ao_sample_pitch; gyro_t ao_sample_yaw; @@ -53,6 +61,9 @@ angle_t ao_sample_orient; angle_t ao_sample_orients[AO_NUM_ORIENT]; uint8_t ao_sample_orient_pos; #endif +#if HAS_MOTOR_PRESSURE +motor_pressure_t ao_sample_motor_pressure; +#endif uint8_t ao_sample_data; @@ -60,8 +71,10 @@ uint8_t ao_sample_data; * Sensor calibration values */ +#if HAS_BARO pres_t ao_ground_pres; /* startup pressure */ alt_t ao_ground_height; /* MSL of ao_ground_pres */ +#endif #if HAS_ACCEL accel_t ao_ground_accel; /* startup acceleration */ @@ -69,31 +82,45 @@ accel_t ao_accel_2g; /* factory accel calibration */ int32_t ao_accel_scale; /* sensor to m/s² conversion */ #endif -#if HAS_GYRO +#if HAS_IMU accel_t ao_ground_accel_along; accel_t ao_ground_accel_across; accel_t ao_ground_accel_through; +#endif + +#if HAS_GYRO int32_t ao_ground_pitch; int32_t ao_ground_yaw; 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; +#if HAS_BARO int32_t ao_sample_pres_sum; +#endif #if HAS_ACCEL int32_t ao_sample_accel_sum; #endif -#if HAS_GYRO +#if HAS_IMU int32_t ao_sample_accel_along_sum; int32_t ao_sample_accel_across_sum; int32_t ao_sample_accel_through_sum; +#endif +#if HAS_GYRO int32_t ao_sample_pitch_sum; int32_t ao_sample_yaw_sum; 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; @@ -105,14 +132,21 @@ ao_sample_preflight_add(void) #if HAS_ACCEL ao_sample_accel_sum += ao_sample_accel; #endif +#if HAS_BARO ao_sample_pres_sum += ao_sample_pres; -#if HAS_GYRO +#endif +#if HAS_IMU ao_sample_accel_along_sum += ao_sample_accel_along; ao_sample_accel_across_sum += ao_sample_accel_across; ao_sample_accel_through_sum += ao_sample_accel_through; +#endif +#if HAS_GYRO 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; } @@ -131,7 +165,7 @@ static void ao_sample_set_one_orient(void) { ao_sample_orients[ao_sample_orient_pos] = ao_sample_orient; - ao_sample_orient_pos = (ao_sample_orient_pos + 1) % AO_NUM_ORIENT; + ao_sample_orient_pos = (uint8_t) ((ao_sample_orient_pos + 1) % AO_NUM_ORIENT); } static void @@ -160,7 +194,7 @@ ao_sample_compute_orient(void) float rotz; rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; - ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); + ao_sample_orient = (angle_t) (acosf(rotz) * (float) (180.0/M_PI)); } #endif /* HAS_GYRO */ @@ -168,22 +202,30 @@ static void ao_sample_preflight_set(void) { #if HAS_ACCEL - ao_ground_accel = ao_sample_accel_sum >> 9; + ao_ground_accel = (accel_t) (ao_sample_accel_sum >> 9); ao_sample_accel_sum = 0; #endif +#if HAS_BARO ao_ground_pres = ao_sample_pres_sum >> 9; ao_ground_height = pres_to_altitude(ao_ground_pres); ao_sample_pres_sum = 0; +#endif +#if HAS_IMU + ao_ground_accel_along = (accel_t) (ao_sample_accel_along_sum >> 9); + ao_ground_accel_across = (accel_t) (ao_sample_accel_across_sum >> 9); + ao_ground_accel_through = (accel_t) (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 = (motor_pressure_t) (ao_sample_motor_pressure_sum >> 9); + ao_sample_motor_pressure_sum = 0; +#endif #if HAS_GYRO - 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_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; @@ -214,9 +256,14 @@ ao_sample_preflight_set(void) */ ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); #if HAS_FLIGHT_DEBUG - if (ao_orient_test) - printf("\n\treset\n"); -#endif + if (ao_orient_test) { + printf("\n\treset across %d through %d along %d\n", + (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)); + fflush(stdout); + } +#endif ao_sample_compute_orient(); ao_sample_set_all_orients(); @@ -225,14 +272,13 @@ ao_sample_preflight_set(void) } #if HAS_GYRO - #define TIME_DIV 200.0f static void ao_sample_rotate(void) { #ifdef AO_FLIGHT_TEST - float dt = (int16_t) (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; + float dt = (AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; #else static const float dt = 1/TIME_DIV; #endif @@ -254,6 +300,7 @@ ao_sample_rotate(void) (int) (y * 1000), (int) (z * 1000), ao_sample_orient); + fflush(stdout); } #endif ao_sample_compute_orient(); @@ -297,6 +344,10 @@ ao_sample_preflight_update(void) ++nsamples; else ao_sample_preflight_set(); +#if !HAS_BARO && HAS_KALMAN + if ((nsamples & 0x3f) == 0) + ao_kalman_reset_accumulate(); +#endif } #if 0 @@ -338,26 +389,30 @@ ao_sample(void) #endif #if HAS_ACCEL - ao_sample_accel = ao_data_accel_cook(ao_data); - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - ao_sample_accel = ao_data_accel_invert(ao_sample_accel); - ao_data_set_accel(ao_data, ao_sample_accel); + ao_sample_accel = ao_data_accel(ao_data); #endif -#if HAS_GYRO +#if HAS_IMU ao_sample_accel_along = ao_data_along(ao_data); ao_sample_accel_across = ao_data_across(ao_data); ao_sample_accel_through = ao_data_through(ao_data); +#endif +#if HAS_GYRO ao_sample_pitch = ao_data_pitch(ao_data); 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(); else { if (ao_flight_state < ao_flight_boost) ao_sample_preflight_update(); +#if HAS_KALMAN ao_kalman(); +#endif #if HAS_GYRO ao_sample_rotate(); #endif @@ -375,19 +430,23 @@ ao_sample_init(void) { ao_config_get(); nsamples = 0; +#if HAS_BARO ao_sample_pres_sum = 0; ao_sample_pres = 0; +#endif #if HAS_ACCEL ao_sample_accel_sum = 0; ao_sample_accel = 0; #endif -#if HAS_GYRO +#if HAS_IMU ao_sample_accel_along_sum = 0; ao_sample_accel_across_sum = 0; ao_sample_accel_through_sum = 0; ao_sample_accel_along = 0; ao_sample_accel_across = 0; ao_sample_accel_through = 0; +#endif +#if HAS_GYRO ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0;