diff options
| -rw-r--r-- | src/test/ao_flight_test.c | 233 | ||||
| -rwxr-xr-x | src/test/plotmm | 8 |
2 files changed, 118 insertions, 123 deletions
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 452f5b75..0abb4090 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -92,6 +92,95 @@ struct ao_adc { #include <ao_data.h> #include <ao_log.h> +#include <ao_telemetry.h> + +#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)) @@ -390,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) @@ -457,6 +547,8 @@ ao_insert(void) 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; @@ -464,6 +556,7 @@ ao_insert(void) 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], @@ -471,7 +564,15 @@ ao_insert(void) 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", @@ -535,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) @@ -771,6 +753,17 @@ ao_sleep(void *wchan) 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) { diff --git a/src/test/plotmm b/src/test/plotmm index bfe15f4c..27f8ddcd 100755 --- a/src/test/plotmm +++ b/src/test/plotmm @@ -15,13 +15,15 @@ case $# in esac gnuplot -persist << EOF -set ylabel "altitude (m)" +set ylabel "distance (m)" set y2label "angle (d)" set xlabel "time (s)" set xtics border out nomirror set ytics border out nomirror set y2tics border out nomirror set title "$title" -plot "$file" using 1:3 with lines axes x1y1 title "raw height",\ -"$file" using 1:7 with lines axes x1y2 title "angle" +plot "$file" using 1:5 with lines axes x1y1 title "height",\ +"$file" using 1:7 with lines axes x1y2 title "angle",\ +"$file" using 1:13 with lines axes x1y2 title "gps angle",\ +"$file" using 1:15 with lines axes x1y2 title "sats" EOF |