X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=3995d98d199b721b8f1f1cf7be2737e40d320bb5;hp=952a811af2f27e24facc8f5776e40fde0900dfdc;hb=161ae96f9ec11e2586df07c0f6d724ddc4dad76c;hpb=5d9e715d570b24ac124c30772b11923bd26ed670 diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 952a811a..3995d98d 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -48,6 +48,7 @@ int ao_gps_new; #define HAS_MS5607 1 #define HAS_MPU6000 1 #define HAS_MMA655X 1 +#define HAS_HMC5883 1 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -91,6 +92,95 @@ struct ao_adc { #include #include +#include + +#if TELEMEGA +int ao_gps_count; +struct ao_telemetry_location ao_gps_first; +struct ao_telemetry_location ao_gps_prev; +struct ao_telemetry_location ao_gps_static; + +struct ao_telemetry_satellite ao_gps_tracking; + +static inline double sqr(double a) { return a * a; } + +void +cc_great_circle (double start_lat, double start_lon, + double end_lat, double end_lon, + double *dist, double *bearing) +{ + const double rad = M_PI / 180; + const double earth_radius = 6371.2 * 1000; /* in meters */ + double lat1 = rad * start_lat; + double lon1 = rad * -start_lon; + double lat2 = rad * end_lat; + double lon2 = rad * -end_lon; + +// double d_lat = lat2 - lat1; + double d_lon = lon2 - lon1; + + /* From http://en.wikipedia.org/wiki/Great-circle_distance */ + double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) + + sqr(cos(lat1) * sin(lat2) - + sin(lat1) * cos(lat2) * cos(d_lon))); + double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon); + double d = atan2(vdn,vdd); + double course; + + if (cos(lat1) < 1e-20) { + if (lat1 > 0) + course = M_PI; + else + course = -M_PI; + } else { + if (d < 1e-10) + course = 0; + else + course = acos((sin(lat2)-sin(lat1)*cos(d)) / + (sin(d)*cos(lat1))); + if (sin(lon2-lon1) > 0) + course = 2 * M_PI-course; + } + *dist = d * earth_radius; + *bearing = course * 180/M_PI; +} + +double +ao_distance_from_pad(void) +{ + double dist, bearing; + if (!ao_gps_count) + return 0; + + cc_great_circle(ao_gps_first.latitude / 1e7, + ao_gps_first.longitude / 1e7, + ao_gps_static.latitude / 1e7, + ao_gps_static.longitude / 1e7, + &dist, &bearing); + return dist; +} + +double +ao_gps_angle(void) +{ + double dist, bearing; + double height; + double angle; + + if (ao_gps_count < 2) + return 0; + + cc_great_circle(ao_gps_prev.latitude / 1e7, + ao_gps_prev.longitude / 1e7, + ao_gps_static.latitude / 1e7, + ao_gps_static.longitude / 1e7, + &dist, &bearing); + height = ao_gps_static.altitude - ao_gps_prev.altitude; + + angle = atan2(dist, height); + return angle * 180/M_PI; +} +#endif #define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) @@ -349,6 +439,23 @@ ao_test_exit(void) exit(0); } +#ifdef TELEMEGA +struct ao_azel { + int az; + int el; +}; + +static void +azel (struct ao_azel *r, struct ao_quaternion *q) +{ + double v; + + r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5); + v = sqrt (q->x*q->x + q->y*q->y); + r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5); +} +#endif + void ao_insert(void) { @@ -372,6 +479,7 @@ ao_insert(void) double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; #endif + (void) accel; if (!tick_offset) tick_offset = -ao_data_static.tick; if ((prev_tick - ao_data_static.tick) > 0x400) @@ -402,10 +510,97 @@ ao_insert(void) } if (!ao_summary) { +#if TELEMEGA + static struct ao_quaternion ao_ground_mag; + static int ao_ground_mag_set; + + if (!ao_ground_mag_set) { + ao_quaternion_init_vector (&ao_ground_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag); + ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation); + ao_ground_mag_set = 1; + } + + struct ao_quaternion ao_mag, ao_mag_rot; + + ao_quaternion_init_vector(&ao_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + + ao_quaternion_normalize(&ao_mag, &ao_mag); + ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation); + + float ao_dot; + int ao_mag_angle; + + ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag); + + struct ao_azel ground_azel, mag_azel, rot_azel; + + azel(&ground_azel, &ao_ground_mag); + azel(&mag_azel, &ao_mag); + azel(&rot_azel, &ao_mag_rot); + + ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5); + + (void) ao_mag_angle; + + static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 }; + struct ao_quaternion ao_out; + + ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); + + int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); + +#if 0 + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + mag_azel.el, + mag_azel.az); +#endif + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + ao_distance_from_pad(), + (int) floor (ao_gps_angle() + 0.5), + (ao_gps_static.flags & 0xf) * 10); + +#if 0 + printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", + ao_state_names[ao_flight_state], + ground_azel.az, ground_azel.el, + mag_azel.az, mag_azel.el, + rot_azel.az, rot_azel.el, + ground_azel.el - rot_azel.el, + ground_azel.az - rot_azel.az, + ao_mag_angle, + ao_sample_orient, + ao_ground_mag.x, + ao_ground_mag.y, + ao_ground_mag.z, + ao_mag.x, + ao_mag.y, + ao_mag.z, + ao_mag_rot.x, + ao_mag_rot.y, + ao_mag_rot.z); +#endif +#endif + +#if 0 printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA "angle %5d " -/* "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */ + "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", time, @@ -413,14 +608,17 @@ ao_insert(void) accel, #if TELEMEGA ao_sample_orient, -/* + ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), -*/ + ao_data_static.hmc5883.x, + ao_data_static.hmc5883.y, + ao_data_static.hmc5883.z, + ao_mag_angle, #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -430,6 +628,7 @@ ao_insert(void) drogue_height, main_height, ao_error_h_sq_avg); +#endif // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); @@ -437,125 +636,6 @@ ao_insert(void) } } -#define AO_MAX_CALLSIGN 8 -#define AO_MAX_VERSION 8 -#define AO_MAX_TELEMETRY 128 - -struct ao_telemetry_generic { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t payload[27]; /* 5 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 -#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 -#define AO_TELEMETRY_SENSOR_TELENANO 0x03 - -struct ao_telemetry_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 accelerometer (TM only) */ - int16_t pres; /* 8 pressure sensor */ - int16_t temp; /* 10 temperature sensor */ - int16_t v_batt; /* 12 battery voltage */ - int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ - int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int16_t ground_pres; /* 24 average pres on pad */ - int16_t ground_accel; /* 26 average accel on pad */ - int16_t accel_plus_g; /* 28 accel calibration at +1g */ - int16_t accel_minus_g; /* 30 accel calibration at -1g */ - /* 32 */ -}; - -#define AO_TELEMETRY_CONFIGURATION 0x04 - -struct ao_telemetry_configuration { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t device; /* 5 device type */ - uint16_t flight; /* 6 flight number */ - uint8_t config_major; /* 8 Config major version */ - uint8_t config_minor; /* 9 Config minor version */ - uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ - uint16_t main_deploy; /* 12 Main deploy alt in meters */ - uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ - char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ - char version[AO_MAX_VERSION]; /* 24 Software version */ - /* 32 */ -}; - -#define AO_TELEMETRY_LOCATION 0x05 - -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' - -struct ao_telemetry_location { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ - int32_t latitude; /* 8 latitude (degrees * 10⁷) */ - int32_t longitude; /* 12 longitude (degrees * 10⁷) */ - uint8_t year; /* 16 (- 2000) */ - uint8_t month; /* 17 (1-12) */ - uint8_t day; /* 18 (1-31) */ - uint8_t hour; /* 19 (0-23) */ - uint8_t minute; /* 20 (0-59) */ - uint8_t second; /* 21 (0-59) */ - uint8_t pdop; /* 22 (m * 5) */ - uint8_t hdop; /* 23 (m * 5) */ - uint8_t vdop; /* 24 (m * 5) */ - uint8_t mode; /* 25 */ - uint16_t ground_speed; /* 26 cm/s */ - int16_t climb_rate; /* 28 cm/s */ - uint8_t course; /* 30 degrees / 2 */ - uint8_t unused[1]; /* 31 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SATELLITE 0x06 - -struct ao_telemetry_satellite_info { - uint8_t svid; - uint8_t c_n_1; -}; - -struct ao_telemetry_satellite { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t channels; /* 5 number of reported sats */ - - struct ao_telemetry_satellite_info sats[12]; /* 6 */ - uint8_t unused[2]; /* 30 */ - /* 32 */ -}; - -union ao_telemetry_all { - struct ao_telemetry_generic generic; - struct ao_telemetry_sensor sensor; - struct ao_telemetry_configuration configuration; - struct ao_telemetry_location location; - struct ao_telemetry_satellite satellite; -}; uint16_t uint16(uint8_t *bytes, int off) @@ -652,6 +732,18 @@ ao_sleep(void *wchan) ao_flight_started = 1; ao_ground_pres = int32(bytes, 4); ao_ground_height = ao_pa_to_altitude(ao_ground_pres); + ao_ground_accel_along = int16(bytes, 8); + ao_ground_accel_across = int16(bytes, 10); + ao_ground_accel_through = int16(bytes, 12); + ao_ground_roll = int16(bytes, 14); + ao_ground_pitch = int16(bytes, 16); + ao_ground_yaw = int16(bytes, 18); + ao_ground_mpu6000.accel_x = ao_ground_accel_across; + ao_ground_mpu6000.accel_y = ao_ground_accel_along; + ao_ground_mpu6000.accel_z = ao_ground_accel_through; + ao_ground_mpu6000.gyro_x = ao_ground_pitch >> 9; + ao_ground_mpu6000.gyro_y = ao_ground_roll >> 9; + ao_ground_mpu6000.gyro_z = ao_ground_yaw >> 9; break; case 'A': ao_data_static.tick = tick; @@ -659,17 +751,31 @@ ao_sleep(void *wchan) ao_data_static.ms5607_raw.temp = int32(bytes, 4); ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); ao_data_static.mpu6000.accel_x = int16(bytes, 8); - ao_data_static.mpu6000.accel_y = -int16(bytes, 10); + ao_data_static.mpu6000.accel_y = int16(bytes, 10); ao_data_static.mpu6000.accel_z = int16(bytes, 12); ao_data_static.mpu6000.gyro_x = int16(bytes, 14); - ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); + ao_data_static.mpu6000.gyro_y = int16(bytes, 16); ao_data_static.mpu6000.gyro_z = int16(bytes, 18); + ao_data_static.hmc5883.x = int16(bytes, 20); + ao_data_static.hmc5883.y = int16(bytes, 22); + ao_data_static.hmc5883.z = int16(bytes, 24); #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif ao_records_read++; ao_insert(); return; + case 'G': + ao_gps_prev = ao_gps_static; + ao_gps_static.tick = tick; + ao_gps_static.latitude = int32(bytes, 0); + ao_gps_static.longitude = int32(bytes, 4); + ao_gps_static.altitude = int32(bytes, 8); + ao_gps_static.flags = bytes[13]; + if (!ao_gps_count) + ao_gps_first = ao_gps_static; + ao_gps_count++; + break; } continue; } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) { @@ -697,7 +803,7 @@ ao_sleep(void *wchan) for (i = 2; i < nword; i++) { for (j = 0; j < NUM_PYRO_VALUES; j++) - if (!strcmp (words[2], ao_pyro_values[j].name)) + if (!strcmp (words[i], ao_pyro_values[j].name)) break; if (j == NUM_PYRO_VALUES) continue;