From 3cdebeb7d5811da765dcf64be72c359addea6c69 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 2 Jun 2011 14:45:45 +0200 Subject: [PATCH] add a new message comming from the generic_com module and parsed by the server --- conf/messages.xml | 18 +- sw/airborne/modules/com/generic_com.c | 4 +- sw/ground_segment/tmtc/fw_server.ml | 20 + sw/in_progress/satcom/Makefile | 2 + sw/in_progress/satcom/tcp2ivy.c | 910 ++++++++++++------------ sw/in_progress/satcom/tcp2ivy_generic.c | 269 +++++++ 6 files changed, 761 insertions(+), 462 deletions(-) create mode 100644 sw/in_progress/satcom/tcp2ivy_generic.c diff --git a/conf/messages.xml b/conf/messages.xml index 0009e3d02f4..fc430e03e78 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -528,11 +528,19 @@ - - - - - + + + + + + + + + + + + + diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 59cf0f5f229..5ec6240cc75 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -76,8 +76,8 @@ void generic_com_periodic( void ) { FillBufWith16bit(com_trans.buf, 11, gps_gspeed); // ground speed FillBufWith16bit(com_trans.buf, 13, gps_course); // course FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s) - com_trans.buf[17] = electrical.vsupply; - com_trans.buf[18] = (uint8_t)(energy*10); + com_trans.buf[17] = electrical.vsupply; // decivolts + com_trans.buf[18] = (uint8_t)(energy/100); // deciAh com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); com_trans.buf[20] = pprz_mode; com_trans.buf[21] = nav_block; diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml index 4c1a4f74674..af07a3ee51f 100644 --- a/sw/ground_segment/tmtc/fw_server.ml +++ b/sw/ground_segment/tmtc/fw_server.ml @@ -276,6 +276,26 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> and alt = ivalue "alt" in let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in update_waypoint a (ivalue "wp_id") geo (float alt /. 100.) + | "GENERIC_COM" -> + let flight_time = ivalue "flight_time" in + if flight_time >= a.flight_time then begin + a.flight_time <- flight_time; + let lat = fvalue "lat" + and lon = fvalue "lon" in + let geo = make_geo_deg (lat /. 1e7) (lon /. 1e7) in + a.pos <- geo; + a.alt <- fvalue "alt"; + a.gspeed <- fvalue "gspeed" /. 100.; + a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.)); + if !heading_from_course then + a.heading <- a.course; + a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0); + a.bat <- fvalue "vsupply" /. 10.; + a.energy <- ivalue "energy" * 100; + a.throttle <- fvalue "throttle"; + a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE"; + a.cur_block <- ivalue "nav_block"; + end | "FORMATION_SLOT_TM" -> Dl_Pprz.message_send "ground_dl" "FORMATION_SLOT" values | "FORMATION_STATUS_TM" -> diff --git a/sw/in_progress/satcom/Makefile b/sw/in_progress/satcom/Makefile index 269da957cb4..339fb510017 100644 --- a/sw/in_progress/satcom/Makefile +++ b/sw/in_progress/satcom/Makefile @@ -6,6 +6,8 @@ udp2tcp: udp2tcp.c gcc -g -O2 -Wall -o $@ $^ tcp2ivy: tcp2ivy.c gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy -lm +tcp2ivy_generic: tcp2ivy_generic.c + gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -I../../../var/${AIRCRAFT} -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy -lm clean: rm email2udp udp2tcp tcp2ivy diff --git a/sw/in_progress/satcom/tcp2ivy.c b/sw/in_progress/satcom/tcp2ivy.c index 398065559f4..e6e65712c10 100644 --- a/sw/in_progress/satcom/tcp2ivy.c +++ b/sw/in_progress/satcom/tcp2ivy.c @@ -1,455 +1,455 @@ -/* - * Paparazzi tcp to ivy handling for sat based telemetry - * - * Copyright (C) 2011 Martin Mueller - * - * This file is part of paparazzi. - * - * 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; either version 2, 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 - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define AC_ID 161 -unsigned char md5[] = {"\207\151\313\256\355\252\016\273\072\126\273\222\017\372\320\200"}; - -#define PORT_OUT 7024 -#define HOSTADDR "localhost" -#define BUFSIZE 0x10000 - -#define TIMEOUT_PERIOD 5000 - -#define MAX_PPRZ 9600 - -#define RadOfDeg(x) ((x) * (M_PI/180.)) - -static const char usage_str[] = -"tcp2ivy [options]\n" -"options:\n" -" -s \n"; - -unsigned char gps_mode; -unsigned short gps_week; -unsigned int gps_itow; -unsigned int gps_alt; -unsigned short gps_gspeed; -short gps_climb; -short gps_course; -int gps_utm_east, gps_utm_north; -unsigned char gps_utm_zone; -int gps_lat, gps_lon; /* 1e7 deg */ -int gps_hmsl; -short estimator_airspeed; -unsigned char electrical_vsupply; -unsigned char nav_block; -unsigned char energy; -unsigned char throttle; -unsigned short estimator_flight_time; -unsigned char nav_utm_zone0; -float latlong_utm_x, latlong_utm_y; -unsigned char pprz_mode; - - -GMainLoop *ml; -int sock, length; -struct sockaddr_in addr; -char hostaddr[4096] = {HOSTADDR}; - -unsigned int buf2uint(char* dat) -{ - return (unsigned char)*(dat+0) | - ((unsigned char)*(dat+1) << 8) | - ((unsigned char)*(dat+2) << 16)| - ((unsigned char)*(dat+3) << 24); -} - -unsigned short buf2ushort(char* dat) -{ - return (unsigned char)*(dat+0) | - ((unsigned char)*(dat+1) << 8); -} - -static const float serie_coeff_proj_mercator[5] = { - 0.99832429842242842444, - 0.00083632803657738403, - 0.00000075957783563707, - 0.00000000119563131778, - 0.00000000000241079916 -}; - -static const float serie_coeff_proj_mercator_inverse[5] = { - 0.998324298422428424, - 0.000837732168742475825, - 5.90586914811817062e-08, - 1.6734091890305064e-10, - 2.13883575853313883e-13 -}; - -/* Computation for the WGS84 geoid only */ -#define E 0.08181919106 -#define K0 0.9996 -#define XS 500000. -#define YS 0. -#define A 6378137.0 -#define N (K0*A) - -struct complex { float re; float im; }; - -#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) -#define CScal(k, z) { z.re *= k; z.im *= k; } -#define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; } -#define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; } -#define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; } -#define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); } -#define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); } - -static inline float isometric_latitude(float phi, float e) { - return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); -} - -static inline float isometric_latitude0(float phi) { - return log (tan (M_PI_4 + phi / 2.0)); -} - -void latlong_utm_of(float phi, float lambda, uint8_t utm_zone) { - float lambda_c = LambdaOfUtmZone(utm_zone); - float ll = isometric_latitude(phi , E); - float dl = lambda - lambda_c; - float phi_ = asin(sin(dl) / cosh(ll)); - float ll_ = isometric_latitude0(phi_); - float lambda_ = atan(sinh(ll) / cos(dl)); - struct complex z_ = { lambda_, ll_ }; - CScal(serie_coeff_proj_mercator[0], z_); - uint8_t k; - for(k = 1; k < 3; k++) { - struct complex z = { lambda_, ll_ }; - CScal(2*k, z); - CSin(z); - CScal(serie_coeff_proj_mercator[k], z); - CAdd(z, z_); - } - CScal(N, z_); - latlong_utm_x = XS + z_.im; - latlong_utm_y = z_.re; -} - -static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { - int count; - char buf[BUFSIZE]; - - /* receive data packet containing formatted data */ - count = recv(sock, buf, sizeof(buf), 0); - if (count > 0) { - if (count == 23) { -// FillBufWith32bit(com_trans.buf, 1, gps_lat); - gps_lat = buf2uint(&buf[0]); -// FillBufWith32bit(com_trans.buf, 5, gps_lon); - gps_lon = buf2uint(&buf[4]); -// FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps_alt/100)); // meters - gps_alt = buf2ushort(&buf[8]) * 100; -// FillBufWith16bit(com_trans.buf, 11, gps_gspeed); // ground speed - gps_gspeed = buf2ushort(&buf[10]); -// FillBufWith16bit(com_trans.buf, 13, gps_course); // course - gps_course = buf2ushort(&buf[12]); -// FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s) - estimator_airspeed = buf2ushort(&buf[14]); -// com_trans.buf[16] = electrical.vsupply; -// should be (estimator_airspeed is two bytes): -// com_trans.buf[17] = electrical.vsupply; - electrical_vsupply = buf[16]; -// com_trans.buf[17] = (uint8_t)(energy*10); - energy = buf[17] / 10; -// com_trans.buf[18] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); - throttle = buf[18]; -// com_trans.buf[19] = pprz_mode; - pprz_mode = buf[19]; -// com_trans.buf[20] = nav_block; - nav_block = buf[20]; -// FillBufWith16bit(com_trans.buf, 21, estimator_flight_time); - estimator_flight_time = buf2ushort(&buf[21]); - -//gps_lat = 52.2648312 * 1e7; -//gps_lon = 9.9939456 * 1e7; -//gps_alt = 169 * 1000; - -//gps_gspeed = 13 * 100; -//gps_course = 60 * 10; -//estimator_airspeed = 15 * 100; -//electrical_vsupply = 126; -//energy = 9; -//throttle = 51; -//pprz_mode = 2; -//nav_block = 1; -//estimator_flight_time = 123; - - nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1; - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - gps_utm_zone = nav_utm_zone0; - -printf("gps_lat %f\n", gps_lat/1e7); -printf("gps_lon %f\n", gps_lon/1e7); -printf("gps_alt %d\n", gps_alt); -printf("gps_gspeed %d\n", gps_gspeed); -printf("gps_course %d\n", gps_course); -printf("estimator_airspeed %d\n", estimator_airspeed); -printf("electrical_vsupply %d\n", electrical_vsupply); -printf("energy %d\n", energy); -printf("throttle %d\n", throttle); -printf("pprz_mode %d\n", pprz_mode); -printf("nav_block %d\n", nav_block); -printf("estimator_flight_time %d\n", estimator_flight_time); - -printf("gps_utm_east %d\n", gps_utm_east); -printf("gps_utm_north %d\n", gps_utm_north); -printf("gps_utm_zone %d\n", gps_utm_zone); - -/* - - - - - - - - - - - - - -*/ - IvySendMsg("%d GPS %d %d %d %d %d %d %d %d %d %d %d", - AC_ID, - 3, // mode = 3D - gps_utm_east, - gps_utm_north, - gps_course, - gps_alt, - gps_gspeed, - 0, // climb - 0, // week - 0, //itow - gps_utm_zone, - 0); // gps_nb_err - -/* - - - - - - - - -*/ - IvySendMsg("%d PPRZ_MODE %d %d %d %d %d %d", - AC_ID, - pprz_mode, - 0, // ap_gaz - 0, // ap_lateral - 0, // ap_horizontal - 0, // if_calib_mode - 0); // mcu1_status - -/* - - - - - - -*/ - IvySendMsg("%d AIRSPEED %f %d %d %d", - AC_ID, - (float)(estimator_airspeed / 100.), - 0, // airspeed_sp - 0, // airspeed_cnt - 0); // groundspeed_sp - -/* - - - - - - - - - - -*/ - IvySendMsg("%d BAT %d %d %d %d %d %d %d %d", - AC_ID, - throttle * MAX_PPRZ / 100, - electrical_vsupply, - 0, // amps - estimator_flight_time, - 0, // kill_auto_throttle - 0, // block_time - 0, // stage_time - energy); - -/* - - - - - - - - - - -*/ - IvySendMsg("%d NAVIGATION %d %d %d %d %d %d %d %d", - AC_ID, - nav_block, - 0, // cur_stage - 0, // pos_x - 0, // pos_y - 0, // dist2_wp - 0, // dist2_home - 0, // circle_count - 0); // oval_count - -/* - - - - -*/ - IvySendMsg("%d ESTIMATOR %f %d", - AC_ID, - gps_alt / 1000., - 0); // z_dot - -/* - - - - - -*/ - IvySendMsg("%d ATTITUDE %f %f %f", - AC_ID, - 0., // phi - RadOfDeg(gps_course / 10.), - 0.); // theta - - } - } - else { - printf("disconnect\n"); - close(sock); - g_main_loop_quit(ml); - return 0; - } - - return 1; -} - -static gboolean alive(gpointer data __attribute__ ((unused))) { -/* - - - -*/ - IvySendMsg("%d ALIVE %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,", - AC_ID, - md5[0], md5[1], md5[2], md5[3], - md5[4], md5[5], md5[6], md5[7], - md5[8], md5[9], md5[10], md5[11], - md5[12], md5[13], md5[14], md5[15]); - - return 1; -} - -int main ( int argc, char** argv) { - GIOChannel *sk; - struct hostent *hent; - int c; - - IvyInit ("IvySatCom", "IvySatCom READY", NULL, NULL, NULL, NULL); - IvyStart("127.255.255.255"); - - while ((c = getopt(argc, argv, "h:s:")) != EOF) { - switch (c) { - - case 'h': - printf(usage_str); - break; - - case 's': - strncpy(hostaddr, optarg, strlen(optarg)+1); - break; - } - } - - sock = socket(PF_INET, SOCK_STREAM, 0); - if (sock < 0) { - perror("socket"); - exit(1); - } - - hent = gethostbyname(hostaddr); - if (hent == 0) { - perror("unknown host"); - exit(1); - } - - memset(&addr, 0, sizeof(addr)); - addr.sin_family = AF_INET; - addr.sin_port = htons(PORT_OUT); - memcpy((char *)&addr.sin_addr, - (char *)hent->h_addr_list[0], - 4); - - if (connect(sock, (struct sockaddr *) &addr, sizeof(addr))) { - perror("connect"); - exit(1); - } - - sk = g_io_channel_unix_new(sock); - g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, - read_data, NULL); - g_timeout_add(TIMEOUT_PERIOD, alive, NULL); - - ml = g_main_loop_new(NULL, FALSE); - - g_main_loop_run(ml); - - return 0; -} +/* + * Paparazzi tcp to ivy handling for sat based telemetry + * + * Copyright (C) 2011 Martin Mueller + * + * This file is part of paparazzi. + * + * 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; either version 2, 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 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define AC_ID 161 +unsigned char md5[] = {"\207\151\313\256\355\252\016\273\072\126\273\222\017\372\320\200"}; + +#define PORT_OUT 7024 +#define HOSTADDR "localhost" +#define BUFSIZE 0x10000 + +#define TIMEOUT_PERIOD 5000 + +#define MAX_PPRZ 9600 + +#define RadOfDeg(x) ((x) * (M_PI/180.)) + +static const char usage_str[] = +"tcp2ivy [options]\n" +"options:\n" +" -s \n"; + +unsigned char gps_mode; +unsigned short gps_week; +unsigned int gps_itow; +unsigned int gps_alt; +unsigned short gps_gspeed; +short gps_climb; +short gps_course; +int gps_utm_east, gps_utm_north; +unsigned char gps_utm_zone; +int gps_lat, gps_lon; /* 1e7 deg */ +int gps_hmsl; +short estimator_airspeed; +unsigned char electrical_vsupply; +unsigned char nav_block; +unsigned char energy; +unsigned char throttle; +unsigned short estimator_flight_time; +unsigned char nav_utm_zone0; +float latlong_utm_x, latlong_utm_y; +unsigned char pprz_mode; + + +GMainLoop *ml; +int sock, length; +struct sockaddr_in addr; +char hostaddr[4096] = {HOSTADDR}; + +unsigned int buf2uint(char* dat) +{ + return (unsigned char)*(dat+0) | + ((unsigned char)*(dat+1) << 8) | + ((unsigned char)*(dat+2) << 16)| + ((unsigned char)*(dat+3) << 24); +} + +unsigned short buf2ushort(char* dat) +{ + return (unsigned char)*(dat+0) | + ((unsigned char)*(dat+1) << 8); +} + +static const float serie_coeff_proj_mercator[5] = { + 0.99832429842242842444, + 0.00083632803657738403, + 0.00000075957783563707, + 0.00000000119563131778, + 0.00000000000241079916 +}; + +static const float serie_coeff_proj_mercator_inverse[5] = { + 0.998324298422428424, + 0.000837732168742475825, + 5.90586914811817062e-08, + 1.6734091890305064e-10, + 2.13883575853313883e-13 +}; + +/* Computation for the WGS84 geoid only */ +#define E 0.08181919106 +#define K0 0.9996 +#define XS 500000. +#define YS 0. +#define A 6378137.0 +#define N (K0*A) + +struct complex { float re; float im; }; + +#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) +#define CScal(k, z) { z.re *= k; z.im *= k; } +#define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; } +#define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; } +#define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; } +#define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); } +#define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); } + +static inline float isometric_latitude(float phi, float e) { + return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); +} + +static inline float isometric_latitude0(float phi) { + return log (tan (M_PI_4 + phi / 2.0)); +} + +void latlong_utm_of(float phi, float lambda, uint8_t utm_zone) { + float lambda_c = LambdaOfUtmZone(utm_zone); + float ll = isometric_latitude(phi , E); + float dl = lambda - lambda_c; + float phi_ = asin(sin(dl) / cosh(ll)); + float ll_ = isometric_latitude0(phi_); + float lambda_ = atan(sinh(ll) / cos(dl)); + struct complex z_ = { lambda_, ll_ }; + CScal(serie_coeff_proj_mercator[0], z_); + uint8_t k; + for(k = 1; k < 3; k++) { + struct complex z = { lambda_, ll_ }; + CScal(2*k, z); + CSin(z); + CScal(serie_coeff_proj_mercator[k], z); + CAdd(z, z_); + } + CScal(N, z_); + latlong_utm_x = XS + z_.im; + latlong_utm_y = z_.re; +} + +static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { + int count; + char buf[BUFSIZE]; + + /* receive data packet containing formatted data */ + count = recv(sock, buf, sizeof(buf), 0); + if (count > 0) { + if (count == 23) { +// FillBufWith32bit(com_trans.buf, 1, gps_lat); + gps_lat = buf2uint(&buf[0]); +// FillBufWith32bit(com_trans.buf, 5, gps_lon); + gps_lon = buf2uint(&buf[4]); +// FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps_alt/100)); // meters + gps_alt = buf2ushort(&buf[8]) * 100; +// FillBufWith16bit(com_trans.buf, 11, gps_gspeed); // ground speed + gps_gspeed = buf2ushort(&buf[10]); +// FillBufWith16bit(com_trans.buf, 13, gps_course); // course + gps_course = buf2ushort(&buf[12]); +// FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s) + estimator_airspeed = buf2ushort(&buf[14]); +// com_trans.buf[16] = electrical.vsupply; +// should be (estimator_airspeed is two bytes): +// com_trans.buf[17] = electrical.vsupply; + electrical_vsupply = buf[16]; +// com_trans.buf[17] = (uint8_t)(energy*10); + energy = buf[17] / 10; +// com_trans.buf[18] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); + throttle = buf[18]; +// com_trans.buf[19] = pprz_mode; + pprz_mode = buf[19]; +// com_trans.buf[20] = nav_block; + nav_block = buf[20]; +// FillBufWith16bit(com_trans.buf, 21, estimator_flight_time); + estimator_flight_time = buf2ushort(&buf[21]); + +//gps_lat = 52.2648312 * 1e7; +//gps_lon = 9.9939456 * 1e7; +//gps_alt = 169 * 1000; + +//gps_gspeed = 13 * 100; +//gps_course = 60 * 10; +//estimator_airspeed = 15 * 100; +//electrical_vsupply = 126; +//energy = 9; +//throttle = 51; +//pprz_mode = 2; +//nav_block = 1; +//estimator_flight_time = 123; + + nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1; + latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); + gps_utm_east = latlong_utm_x * 100; + gps_utm_north = latlong_utm_y * 100; + gps_utm_zone = nav_utm_zone0; + +printf("gps_lat %f\n", gps_lat/1e7); +printf("gps_lon %f\n", gps_lon/1e7); +printf("gps_alt %d\n", gps_alt); +printf("gps_gspeed %d\n", gps_gspeed); +printf("gps_course %d\n", gps_course); +printf("estimator_airspeed %d\n", estimator_airspeed); +printf("electrical_vsupply %d\n", electrical_vsupply); +printf("energy %d\n", energy); +printf("throttle %d\n", throttle); +printf("pprz_mode %d\n", pprz_mode); +printf("nav_block %d\n", nav_block); +printf("estimator_flight_time %d\n", estimator_flight_time); + +printf("gps_utm_east %d\n", gps_utm_east); +printf("gps_utm_north %d\n", gps_utm_north); +printf("gps_utm_zone %d\n", gps_utm_zone); + +/* + + + + + + + + + + + + + +*/ + IvySendMsg("%d GPS %d %d %d %d %d %d %d %d %d %d %d", + AC_ID, + 3, // mode = 3D + gps_utm_east, + gps_utm_north, + gps_course, + gps_alt, + gps_gspeed, + 0, // climb + 0, // week + 0, //itow + gps_utm_zone, + 0); // gps_nb_err + +/* + + + + + + + + +*/ + IvySendMsg("%d PPRZ_MODE %d %d %d %d %d %d", + AC_ID, + pprz_mode, + 0, // ap_gaz + 0, // ap_lateral + 0, // ap_horizontal + 0, // if_calib_mode + 0); // mcu1_status + +/* + + + + + + +*/ + IvySendMsg("%d AIRSPEED %f %d %d %d", + AC_ID, + (float)(estimator_airspeed / 100.), + 0, // airspeed_sp + 0, // airspeed_cnt + 0); // groundspeed_sp + +/* + + + + + + + + + + +*/ + IvySendMsg("%d BAT %d %d %d %d %d %d %d %d", + AC_ID, + throttle * MAX_PPRZ / 100, + electrical_vsupply, + 0, // amps + estimator_flight_time, + 0, // kill_auto_throttle + 0, // block_time + 0, // stage_time + energy); + +/* + + + + + + + + + + +*/ + IvySendMsg("%d NAVIGATION %d %d %d %d %d %d %d %d", + AC_ID, + nav_block, + 0, // cur_stage + 0, // pos_x + 0, // pos_y + 0, // dist2_wp + 0, // dist2_home + 0, // circle_count + 0); // oval_count + +/* + + + + +*/ + IvySendMsg("%d ESTIMATOR %f %d", + AC_ID, + gps_alt / 1000., + 0); // z_dot + +/* + + + + + +*/ + IvySendMsg("%d ATTITUDE %f %f %f", + AC_ID, + 0., // phi + RadOfDeg(gps_course / 10.), + 0.); // theta + + } + } + else { + printf("disconnect\n"); + close(sock); + g_main_loop_quit(ml); + return 0; + } + + return 1; +} + +static gboolean alive(gpointer data __attribute__ ((unused))) { +/* + + + +*/ + IvySendMsg("%d ALIVE %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,", + AC_ID, + md5[0], md5[1], md5[2], md5[3], + md5[4], md5[5], md5[6], md5[7], + md5[8], md5[9], md5[10], md5[11], + md5[12], md5[13], md5[14], md5[15]); + + return 1; +} + +int main ( int argc, char** argv) { + GIOChannel *sk; + struct hostent *hent; + int c; + + IvyInit ("IvySatCom", "IvySatCom READY", NULL, NULL, NULL, NULL); + IvyStart("127.255.255.255"); + + while ((c = getopt(argc, argv, "h:s:")) != EOF) { + switch (c) { + + case 'h': + printf(usage_str); + break; + + case 's': + strncpy(hostaddr, optarg, strlen(optarg)+1); + break; + } + } + + sock = socket(PF_INET, SOCK_STREAM, 0); + if (sock < 0) { + perror("socket"); + exit(1); + } + + hent = gethostbyname(hostaddr); + if (hent == 0) { + perror("unknown host"); + exit(1); + } + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(PORT_OUT); + memcpy((char *)&addr.sin_addr, + (char *)hent->h_addr_list[0], + 4); + + if (connect(sock, (struct sockaddr *) &addr, sizeof(addr))) { + perror("connect"); + exit(1); + } + + sk = g_io_channel_unix_new(sock); + g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, + read_data, NULL); + g_timeout_add(TIMEOUT_PERIOD, alive, NULL); + + ml = g_main_loop_new(NULL, FALSE); + + g_main_loop_run(ml); + + return 0; +} diff --git a/sw/in_progress/satcom/tcp2ivy_generic.c b/sw/in_progress/satcom/tcp2ivy_generic.c new file mode 100644 index 00000000000..21c1f42b8b7 --- /dev/null +++ b/sw/in_progress/satcom/tcp2ivy_generic.c @@ -0,0 +1,269 @@ +/* + * Paparazzi tcp to ivy handling for sat based telemetry + * + * Copyright (C) 2011 Martin Mueller + * + * This file is part of paparazzi. + * + * 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; either version 2, 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 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "generated/airframe.h" + +//#define AC_ID 161 +//unsigned char md5[] = {"\207\151\313\256\355\252\016\273\072\126\273\222\017\372\320\200"}; +unsigned char* md5 = (unsigned char*)MD5SUM; + +#define PORT_OUT 7024 +#define HOSTADDR "localhost" +#define BUFSIZE 0x10000 + +#define TIMEOUT_PERIOD 5000 + +#define MAX_PPRZ 9600 + +#define RadOfDeg(x) ((x) * (M_PI/180.)) + +static const char usage_str[] = +"tcp2ivy [options]\n" +"options:\n" +" -s \n"; + +unsigned char gps_mode; +unsigned short gps_week; +unsigned int gps_itow; +unsigned int gps_alt; +unsigned short gps_gspeed; +short gps_climb; +short gps_course; +int gps_utm_east, gps_utm_north; +unsigned char gps_utm_zone; +int gps_lat, gps_lon; /* 1e7 deg */ +int gps_hmsl; +short estimator_airspeed; +unsigned char electrical_vsupply; +unsigned char nav_block; +unsigned short energy; +unsigned char throttle; +unsigned short estimator_flight_time; +unsigned char nav_utm_zone0; +float latlong_utm_x, latlong_utm_y; +unsigned char pprz_mode; + + +GMainLoop *ml; +int sock, length; +struct sockaddr_in addr; +char hostaddr[4096] = {HOSTADDR}; + +unsigned int buf2uint(char* dat) +{ + return (unsigned char)*(dat+0) | + ((unsigned char)*(dat+1) << 8) | + ((unsigned char)*(dat+2) << 16)| + ((unsigned char)*(dat+3) << 24); +} + +unsigned short buf2ushort(char* dat) +{ + return (unsigned char)*(dat+0) | + ((unsigned char)*(dat+1) << 8); +} + +static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) { + int count; + char buf[BUFSIZE]; + + /* receive data packet containing formatted data */ + count = recv(sock, buf, sizeof(buf), 0); + if (count > 0) { + if (count == 23) { + // FillBufWith32bit(com_trans.buf, 1, gps_lat); + gps_lat = buf2uint(&buf[0]); + // FillBufWith32bit(com_trans.buf, 5, gps_lon); + gps_lon = buf2uint(&buf[4]); + // FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps_alt/100)); // meters + gps_alt = buf2ushort(&buf[8]); + // FillBufWith16bit(com_trans.buf, 11, gps_gspeed); // ground speed + gps_gspeed = buf2ushort(&buf[10]); + // FillBufWith16bit(com_trans.buf, 13, gps_course); // course + gps_course = buf2ushort(&buf[12]); + // FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s) + estimator_airspeed = buf2ushort(&buf[14]); + // com_trans.buf[17] = electrical.vsupply; // decivolt + electrical_vsupply = buf[16]; + // com_trans.buf[18] = (uint8_t)(energy / 100); // deciAh + energy = buf[17]; + // com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); + throttle = buf[18]; + // com_trans.buf[20] = pprz_mode; + pprz_mode = buf[19]; + // com_trans.buf[21] = nav_block; + nav_block = buf[20]; + // FillBufWith16bit(com_trans.buf, 22, estimator_flight_time); + estimator_flight_time = buf2ushort(&buf[21]); + +#if 0 + gps_lat = 52.2648312 * 1e7; + gps_lon = 9.9939456 * 1e7; + gps_alt = 169 * 1000; + gps_gspeed = 13 * 100; + gps_course = 60 * 10; + estimator_airspeed = 15 * 100; + electrical_vsupply = 126; + energy = 9; + throttle = 51; + pprz_mode = 2; + nav_block = 1; + estimator_flight_time = 123; +#endif + + printf("**** message received from iridium module ****\n"); + printf("gps_lat %f\n", gps_lat/1e7); + printf("gps_lon %f\n", gps_lon/1e7); + printf("gps_alt %d\n", gps_alt); + printf("gps_gspeed %d\n", gps_gspeed); + printf("gps_course %d\n", gps_course); + printf("estimator_airspeed %d\n", estimator_airspeed); + printf("electrical_vsupply %d\n", electrical_vsupply); + printf("energy %d\n", energy); + printf("throttle %d\n", throttle); + printf("pprz_mode %d\n", pprz_mode); + printf("nav_block %d\n", nav_block); + printf("estimator_flight_time %d\n", estimator_flight_time); + fflush(stdout); + + IvySendMsg("%d GENERIC_COM %d %d %d %d %d %d %d %d %d %d %d %d", + AC_ID, + gps_lat, + gps_lon, + gps_alt, + gps_gspeed, + gps_course, + estimator_airspeed, + electrical_vsupply, + energy, + throttle, + pprz_mode, + nav_block, + estimator_flight_time); + + } + } + else { + printf("disconnect\n"); + close(sock); + g_main_loop_quit(ml); + return 0; + } + + return 1; +} + +static gboolean alive(gpointer data __attribute__ ((unused))) { + /* + + + + */ + IvySendMsg("%d ALIVE %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,", + AC_ID, + md5[0], md5[1], md5[2], md5[3], + md5[4], md5[5], md5[6], md5[7], + md5[8], md5[9], md5[10], md5[11], + md5[12], md5[13], md5[14], md5[15]); + + return 1; +} + +int main ( int argc, char** argv) { + GIOChannel *sk; + struct hostent *hent; + int c; + + printf("Starting TCP2IVY agent for AC %s (id:%d)\n", AIRFRAME_NAME, AC_ID); + fflush(stdout); + + IvyInit ("IvySatCom", "IvySatCom READY", NULL, NULL, NULL, NULL); + IvyStart("127.255.255.255"); + + while ((c = getopt(argc, argv, "h:s:")) != EOF) { + switch (c) { + + case 'h': + printf(usage_str); + break; + + case 's': + strncpy(hostaddr, optarg, strlen(optarg)+1); + break; + } + } + + sock = socket(PF_INET, SOCK_STREAM, 0); + if (sock < 0) { + perror("socket"); + exit(1); + } + + hent = gethostbyname(hostaddr); + if (hent == 0) { + perror("unknown host"); + exit(1); + } + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(PORT_OUT); + memcpy((char *)&addr.sin_addr, + (char *)hent->h_addr_list[0], + 4); + + if (connect(sock, (struct sockaddr *) &addr, sizeof(addr))) { + perror("connect"); + exit(1); + } + + sk = g_io_channel_unix_new(sock); + g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, + read_data, NULL); + g_timeout_add(TIMEOUT_PERIOD, alive, NULL); + + ml = g_main_loop_new(NULL, FALSE); + + g_main_loop_run(ml); + + return 0; +}