X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_sample.c;h=e3b5e084d8f4c52ec454e30739910bfada8a9912;hp=90ea07adfa6843246dc6d3d52a14ad0298d5f0c3;hb=7f46240dfc57164f0c1b0c4c4ed9695bca63860d;hpb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 90ea07ad..e3b5e084 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -35,63 +35,88 @@ #define ACCEL_TYPE int16_t #endif -__pdata uint16_t ao_sample_tick; /* time of last data */ -__pdata pres_t ao_sample_pres; -__pdata alt_t ao_sample_alt; -__pdata alt_t ao_sample_height; +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 -__pdata accel_t ao_sample_accel; +accel_t ao_sample_accel; +#endif +#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 -__pdata accel_t ao_sample_accel_along; -__pdata accel_t ao_sample_accel_across; -__pdata accel_t ao_sample_accel_through; -__pdata gyro_t ao_sample_roll; -__pdata gyro_t ao_sample_pitch; -__pdata gyro_t ao_sample_yaw; -__pdata angle_t ao_sample_orient; +gyro_t ao_sample_roll; +gyro_t ao_sample_pitch; +gyro_t ao_sample_yaw; +angle_t ao_sample_orient; +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 -__data uint8_t ao_sample_data; +uint8_t ao_sample_data; /* * Sensor calibration values */ -__pdata pres_t ao_ground_pres; /* startup pressure */ -__pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ +#if HAS_BARO +pres_t ao_ground_pres; /* startup pressure */ +alt_t ao_ground_height; /* MSL of ao_ground_pres */ +#endif #if HAS_ACCEL -__pdata accel_t ao_ground_accel; /* startup acceleration */ -__pdata accel_t ao_accel_2g; /* factory accel calibration */ -__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ +accel_t ao_ground_accel; /* startup acceleration */ +accel_t ao_accel_2g; /* factory accel calibration */ +int32_t ao_accel_scale; /* sensor to m/s² conversion */ +#endif + +#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 -__pdata accel_t ao_ground_accel_along; -__pdata accel_t ao_ground_accel_across; -__pdata accel_t ao_ground_accel_through; -__pdata int32_t ao_ground_pitch; -__pdata int32_t ao_ground_yaw; -__pdata int32_t ao_ground_roll; +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 __pdata uint8_t ao_preflight; /* in preflight mode */ +static uint8_t ao_preflight; /* in preflight mode */ -static __pdata uint16_t nsamples; -__pdata int32_t ao_sample_pres_sum; +static uint16_t nsamples; +#if HAS_BARO +int32_t ao_sample_pres_sum; +#endif #if HAS_ACCEL -__pdata int32_t ao_sample_accel_sum; +int32_t ao_sample_accel_sum; +#endif +#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 -__pdata int32_t ao_sample_accel_along_sum; -__pdata int32_t ao_sample_accel_across_sum; -__pdata int32_t ao_sample_accel_through_sum; -__pdata int32_t ao_sample_pitch_sum; -__pdata int32_t ao_sample_yaw_sum; -__pdata int32_t ao_sample_roll_sum; +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; @@ -103,18 +128,72 @@ 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; } +#if HAS_GYRO +static void +ao_sample_set_all_orients(void) +{ + int i; + for (i = 0; i < AO_NUM_ORIENT; i++) + ao_sample_orients[i] = ao_sample_orient; + ao_sample_orient_pos = 0; +} + +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; +} + +static void +ao_sample_compute_orient(void) +{ + /* Compute pitch angle from vertical by taking the pad + * orientation vector and rotating it by the current total + * rotation value. That will be a unit vector pointing along + * the airframe axis. The Z value will be the cosine of the + * change in the angle from vertical since boost. + * + * rot = ao_rotation * vertical * ao_rotation° + * rot = ao_rotation * (0,0,0,1) * ao_rotation° + * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) + * = a.z² - a.y² - a.x² + a.r² + * + * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° + * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) + * = -a.z² + a.y² + a.x² - a.r² + */ + + 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); +} +#endif /* HAS_GYRO */ + static void ao_sample_preflight_set(void) { @@ -122,23 +201,31 @@ ao_sample_preflight_set(void) ao_ground_accel = 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; -#if HAS_GYRO +#endif +#if HAS_IMU 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; +#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; ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_orient = 0; + ao_sample_set_all_orients(); struct ao_quaternion orient; @@ -165,28 +252,35 @@ 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(); #endif nsamples = 0; } #if HAS_GYRO - #define TIME_DIV 200.0f static void ao_sample_rotate(void) { #ifdef AO_FLIGHT_TEST - float dt = (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 - float x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; - float y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; - float z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; + float x = ao_convert_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; + float y = ao_convert_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; + float z = ao_convert_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; struct ao_quaternion rot; ao_quaternion_init_half_euler(&rot, x, y, z); @@ -195,31 +289,6 @@ ao_sample_rotate(void) /* And normalize to make sure it remains a unit vector */ ao_quaternion_normalize(&ao_rotation, &ao_rotation); - /* Compute pitch angle from vertical by taking the pad - * orientation vector and rotating it by the current total - * rotation value. That will be a unit vector pointing along - * the airframe axis. The Z value will be the cosine of the - * change in the angle from vertical since boost. - * - * rot = ao_rotation * vertical * ao_rotation° - * rot = ao_rotation * (0,0,0,1) * ao_rotation° - * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) - * = a.z² - a.y² - a.x² + a.r² - * - * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° - * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) - * = -a.z² + a.y² + a.x² - a.r² - */ - - 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); - #if HAS_FLIGHT_DEBUG if (ao_orient_test) { printf ("rot %d %d %d orient %d \r", @@ -227,9 +296,11 @@ ao_sample_rotate(void) (int) (y * 1000), (int) (z * 1000), ao_sample_orient); + fflush(stdout); } #endif - + ao_sample_compute_orient(); + ao_sample_set_one_orient(); } #endif @@ -249,7 +320,7 @@ ao_sample_preflight(void) ao_accel_scale = to_fix_32(GRAVITY * 2 * 16) / ao_accel_2g; #endif ao_sample_preflight_set(); - ao_preflight = FALSE; + ao_preflight = false; } } @@ -269,6 +340,10 @@ ao_sample_preflight_update(void) ++nsamples; else ao_sample_preflight_set(); +#if !HAS_BARO + if ((nsamples & 0x3f) == 0) + ao_kalman_reset_accumulate(); +#endif } #if 0 @@ -293,10 +368,10 @@ static gyro_t inline ao_gyro(void) { uint8_t ao_sample(void) { - ao_wakeup(DATA_TO_XDATA(&ao_sample_data)); - ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); + ao_wakeup(&ao_sample_data); + ao_sleep((void *) &ao_data_head); while (ao_sample_data != ao_data_head) { - __xdata struct ao_data *ao_data; + struct ao_data *ao_data; /* Capture a sample */ ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data]; @@ -310,19 +385,21 @@ 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(); @@ -347,19 +424,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; @@ -367,7 +448,8 @@ ao_sample_init(void) ao_sample_yaw = 0; ao_sample_roll = 0; ao_sample_orient = 0; + ao_sample_set_all_orients(); #endif ao_sample_data = ao_data_head; - ao_preflight = TRUE; + ao_preflight = true; }