/// get the relevant data from the packet and sent it as Ivy message
void decode_and_send_to_ivy() {
// check packet integrity
char expected[] = "LOO";
if (strncmp((char *)packet, expected, 3) != 0) {
fprintf(stderr, "Received packet from the weather station which does not match the expected format\n");
reset_station();
return;
}
// TODO CRC checking (is rather involved for the Davis protocol)
// get relevant data and convert to SI units
// see chapter IX.1 of the protocol definition
float
pstatic_Pa = (packet[7] | packet[8] << 8)*3.386388640341, // original is inches Hg / 1000
temp_degC = ((packet[12] | packet[13] << 8)/10.0 - 32.0)*5.0/9.0, // original is deg F / 10
windspeed_mps = packet[14]*0.44704, // original is miles per hour
winddir_deg = packet[16] | packet[17] << 8,
rel_humidity = -1.; // TODO Get the real humidity value from message
// TODO get the real MD5 for the aircraft id
if (want_alive_msg)
IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
// format has to match declaration in conf/messages.xml
IvySendMsg("%d WEATHER %f %f %f %f %f\n",
ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg, rel_humidity);
}
gboolean timeout_callback(gpointer data) {
for (int i=0; i<20; i++) {
aos_compute_state();
aos_compute_sensors();
#ifndef DISABLE_PROPAGATE
ahrs_propagate();
#endif
#ifndef DISABLE_ACCEL_UPDATE
ahrs_update_accel();
#endif
#ifndef DISABLE_MAG_UPDATE
if (!(i%5)) ahrs_update_mag();
#endif
}
#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
ahrs_impl.gyro_bias.p,
ahrs_impl.gyro_bias.q,
ahrs_impl.gyro_bias.r);
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
struct Int32Rates bias_i;
RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
bias_i.p,
bias_i.q,
bias_i.r);
#endif
IvySendMsg("183 AHRS_EULER %f %f %f",
ahrs_float.ltp_to_imu_euler.phi,
ahrs_float.ltp_to_imu_euler.theta,
ahrs_float.ltp_to_imu_euler.psi);
IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r),
DegOfRad(aos.ltp_to_imu_euler.phi),
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
return TRUE;
}
/// Get the relevant data from the packet and sent it as Ivy message
void decode_and_send_to_ivy() {
// get relevant data and convert to SI units
// contents of the kestrel S message response:
// DT, MG, TR, WS, CW, HW, TP, WC, RH, HI, DP, WB, BP, AL, DA
// s, Mag, True, mph, mph, mph, F, F, %, F, F, F, inHg, ft, ft
//
// where:
//
// DT is the date and time in seconds since 1st January 2000,
// AV is air velocity
// AF is air flow
// MG is the compass magnetic direction
// TR is the compass true direction
// WS is the wind speed
// CW is the crosswind
// HW is the headwind
// TP is the temperature,
// WC is the wind chill,
// RH is the humidity,
// EV is the evaporation rate
// CT is the concrete temperature
// HR, MO (moisture) are the humidity ratio
// HI is the heat index,
// DP is the dew point,
// WB is the wet bulb
// AP is absolute pressure
// BP is the pressure,
// AL is the altitude,
// DA is the density altitude
// AD is air density
// RA is relative air density
float values[NUM_PARAMS];
float pstatic_Pa, temp_degC, windspeed_mps, winddir_deg;
if (want_alive_msg)
IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
if (getValues(packet + MIN_PACKET_LENGTH, values)){
pstatic_Pa = values[BP] * 3386.4; // original is inches Hg
temp_degC = (values[TP] - 32.0) * 5.0/9.0; // original is deg F
windspeed_mps = values[WS] * 0.44704; // original is miles per hour
winddir_deg = values[MG];
// format has to match declaration in conf/messages.xml
IvySendMsg("%d WEATHER %f %f %f %f\n",
ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg);
}
}
static void on_sync_clicked(GtkButton *button, gpointer user_data) {
static uint8_t syncID = 1;
char *msg;
if (asprintf(&msg, "Aircraft %d, video synchronisation %d", airframeID, syncID) > 0) {
say(msg);
free(msg);
}
else {
fprintf(stderr, "Could not allocate mem for msg string\n");
}
IvySendMsg("%d VIDEO_SYNC %d", airframeID, syncID); //TODO : get the current airframe ID
// create a flashing window in full screen for the camera to see
flashWindow = gtk_window_new(GTK_WINDOW_TOPLEVEL);
GdkColor white;
white.red = 0xFFFF;
white.green = 0xFFFF;
white.blue = 0xFFFF;
gtk_widget_modify_bg(flashWindow, GTK_STATE_NORMAL, &white);
gtk_hbox_new(FALSE, 0);
gtk_window_fullscreen((GtkWindow*)flashWindow);
gtk_widget_show_all(flashWindow);
g_timeout_add(100 , __timeout_flashing_window , ml);
syncID++;
}
static void update_gps(struct gps_data_t *gpsdata,
char *message,
size_t len)
{
static double fix_time = 0;
double fix_track = 0;
double fix_speed = 0;
double fix_altitude = 0;
double fix_climb = 0;
if ((isnan(gpsdata->fix.latitude) == 0) &&
(isnan(gpsdata->fix.longitude) == 0) &&
(isnan(gpsdata->fix.time) == 0) &&
(gpsdata->fix.mode >= MODE_2D) &&
(gpsdata->fix.time != fix_time))
{
if (!isnan(gpsdata->fix.track))
fix_track = gpsdata->fix.track;
if (!isnan(gpsdata->fix.speed))
fix_speed = gpsdata->fix.speed;
if (gpsdata->fix.mode >= MODE_3D)
{
if (!isnan(gpsdata->fix.altitude))
fix_altitude = gpsdata->fix.altitude;
if (!isnan(gpsdata->fix.climb))
fix_climb = gpsdata->fix.climb;
}
if (verbose)
printf("sending gps info viy Ivy: lat %g, lon %g, speed %g, course %g, alt %g, climb %g\n",
gpsdata->fix.latitude, gpsdata->fix.longitude, fix_speed, fix_track, fix_altitude, fix_climb);
IvySendMsg("%s %s %s %f %f %f %f %f %f %f %f %f %f %f %d",
MSG_DEST,
MSG_NAME,
MSG_ID, // ac_id
0.0, // roll,
0.0, // pitch,
0.0, // heading
gpsdata->fix.latitude,
gpsdata->fix.longitude,
fix_speed,
fix_track, // course
fix_altitude,
fix_climb,
0.0, // agl
gpsdata->fix.time,
0); // itow
fix_time = gpsdata->fix.time;
}
else
{
if (verbose)
printf("ignoring gps data: lat %f, lon %f, mode %d, time %f\n", gpsdata->fix.latitude,
gpsdata->fix.longitude, gpsdata->fix.mode, gpsdata->fix.time);
}
}
static void on_as_addr_changed (GtkRadioButton *radiobutton, gpointer user_data) {
#ifdef ASTECH
if (!gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)))
return;
guint new_addr = (guint)user_data;
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_TWI_CONTROLLER_ASCTECH_ADDR, new_addr);
#endif
}
/* Called by clicking a button */
void init_river_tracking( GtkWidget *widget, gpointer data )
{
fprintf(stderr, "initializing river tracking...\n");
/* Call function get_next_waypoint (defined in nexcwp.c)
* which returns a waypoint. */
Waypoint new_wp = get_next_waypoint();
IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt);
}
static gboolean periodic(gpointer data __attribute__ ((unused))) {
int roll, pitch;
counter++;
pitch = 0;
roll = STEP_VAL;
if ((counter%6) >=3) roll = -roll;
IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
return TRUE;
}
void request_ac_config(int ac_id_req) {
RequestID++;
IvySendMsg("app_server %d_%d CONFIG_REQ %d" ,ProcessID, RequestID ,ac_id_req );
//AC config requested..
if (verbose) {
printf("AC(id= %d) config requested.\n",ac_id_req);
fflush(stdout);
}
}
// Deuxième partie de la fonction d'envoi d'un SMS
void Reception_SMS_Continue(int handle)
{
char buffer_SMS_recu[250];
strcpy(buffer_SMS_recu, GSM_Line);
Remplissage_SMS(buffer_SMS_recu);
if (Verification(expediteur) == 1)// Le SMS provient bien de l'avion
{
printf("Nouveau message recu de l'avion\n");
/* Stockage du message dans un fichier de log */
Save_SMS_In_Log();
/* Vérification de l'index (pour ne pas en laisser passer un)*/
if (Check_index(index_msg) == 0) /*on a laissé passer qqch*/
{
printf("Attention perte d'un ou plusieurs messages...\n");
}
printf("Contenu du message :%s\n", data);
// Découpage prévu du SMS recu
decoupage(data);
/* Envoi sur le bus Ivy */
IvySendMsg("16 GPS 3 %s %s %s %s %s %s 0 335297960 31 0", extr_gps_utm_east, extr_gps_utm_north, extr_gps_course, extr_gps_alt, extr_gps_gspeed, extr_gps_climb);
IvySendMsg("16 FBW_STATUS 0 1 %s 0",extr_vsupply);
/* Suppression du message de la carte SIM */
Suppr_SMS(handle, index_msg);
}
fflush(stdout);
}
/* Called when aircraft reaches "Block 1" (the second
* block defined in the flight plan)
*/
void start_track(IvyClientPtr app, void *data, int argc, char **argv)
{
fprintf(stderr, ".");
/* Call function get_next_waypoint (defined in nexcwp.c)
* which returns a waypoint. */
Waypoint new_wp = get_next_waypoint();
if(CONTINUE) {
IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt); //Always move waypoint 4
}
/* else we've reached the end (we've seen the "end" waypoint in the
* picture), so don't send any message.
*/
}
static gboolean alive(gpointer data __attribute__ ((unused))) {
/*
<message name="ALIVE" id="2">
<field name="md5sum" type="uint8[]"/>
</message>
*/
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;
}
static int
IvySendCmd(ClientData clientData,
Tcl_Interp *interp,
int argc,
const char **argv)
{
if (argc != 2) {
Tcl_AppendResult(interp, "wrong # of args: \"",
argv[0], " msg\"", (char *) NULL);
return TCL_ERROR;
}
IvySendMsg(argv[1], NULL);
return TCL_OK;
}
static void update_gps(struct gps_data_t *gpsdata,
char *message,
size_t len)
{
static double fix_time = 0;
double fix_track = 0;
double fix_speed = 0;
double fix_altitude = 0;
double fix_climb = 0;
if ((isnan(gpsdata->fix.latitude) == 0) &&
(isnan(gpsdata->fix.longitude) == 0) &&
(isnan(gpsdata->fix.time) == 0) &&
(gpsdata->fix.mode >= MODE_2D) &&
(gpsdata->fix.time != fix_time))
{
if (isnan(gpsdata->fix.track) != 0) fix_track = gpsdata->fix.track;
if (isnan(gpsdata->fix.speed) != 0) fix_speed = gpsdata->fix.speed;
if (gpsdata->fix.mode >= MODE_3D)
{
if (isnan(gpsdata->fix.altitude) != 0) fix_altitude = gpsdata->fix.altitude;
if (isnan(gpsdata->fix.climb) != 0) fix_climb = gpsdata->fix.climb;
}
IvySendMsg("%s %s %s %f %f %f %f %f %f %f %f %f %f %f %d",
MSG_DEST,
MSG_NAME,
MSG_ID, // ac_id
0.0, // roll,
0.0, // pitch,
0.0, // heading
gpsdata->fix.latitude,
gpsdata->fix.longitude,
fix_speed,
fix_track, // course
fix_altitude,
fix_climb,
0.0, // agl
gpsdata->fix.time,
0); // itow
fix_time = gpsdata->fix.time;
}
}
void on_mode_changed (GtkRadioButton *radiobutton, gpointer user_data) {
mode = gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)) ? MANUAL : AUTO;
IvySendMsg("1ME RAW_DATALINK 80 SETTING;0;0;%d", mode);
g_message("Mode changed to %d" , mode);
}
static void on_mode_changed (GtkRadioButton *radiobutton, gpointer user_data) {
if (!gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)))
return;
guint mode = (guint)user_data;
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_MODES_MODE, mode);
}
static void on_as_reverse_button_clicked (GtkWidget *widget, gpointer data) {
#ifdef ASTECH
IvySendMsg("dl DL_SETTING %d %d %d", mb_id, PPRZ_MB_TWI_CONTROLLER_ASCTECH_COMMAND_TYPE, AS_CMD_REVERSE);
#endif
}
gboolean request_ac_list(gpointer data) {
RequestID++;
IvySendMsg("app_server %d_%d AIRCRAFTS_REQ" ,ProcessID, RequestID);
return FALSE;
}
/* callback associated to "Hello" messages */
void textCallback(IvyClientPtr app, void *data, int argc, char **argv)
{
const char* arg = (argc < 1) ? "" : argv[0];
IvySendMsg("Alo ai%s", arg);
}
void on_scale_value_changed (GtkScale *scale, gpointer user_data) {
gfloat cf = gtk_range_get_value(GTK_RANGE(scale));
gint c = round(cf);
g_message("foo %d %f", user_data, c);
IvySendMsg("ME RAW_DATALINK 16 SETTING;%d;0;%d", (gint)user_data, c);
}
/**
* Get the relevant data from the packet and send it as Ivy message
*/
void decode_and_send_to_ivy() {
// get relevant data and convert to SI units
// contents of the kestrel S message response in imperial units:
// DT, MG, TR, WS, CW, HW, TP, WC, RH, HI, DP, WB, BP, AL, DA
// s, Mag, True, mph, mph, mph, F, F, %, F, F, F, inHg, ft, ft
// in metric units:
// s,Mag,True,km/h,km/h,km/h,�C,�C,%,�C,�C,�C,hPa,m,m
//
// Sample packet in metric units:
// S
// > DT,MG,TR,WS,CW,HW,TP,WC,RH,HI,DP,WB,BP,AL,DA
// s,Mag,True,km/h,km/h,km/h,�C,�C,%,�C,�C,�C,hPa,m,m
// 399124138,000,000,0.0,0.0,0.0,22.5,22.5,37.6,21.4,7.3,13.5,931.5,701,1171
// >
//
// where:
//
// DT is the date and time in seconds since 1st January 2000,
// AV is air velocity
// AF is air flow
// MG is the compass magnetic direction
// TR is the compass true direction
// WS is the wind speed
// CW is the crosswind
// HW is the headwind
// TP is the temperature,
// WC is the wind chill,
// RH is the relative humidity,
// EV is the evaporation rate
// CT is the concrete temperature
// HR, MO (moisture) are the humidity ratio
// HI is the heat index,
// DP is the dew point,
// WB is the wet bulb
// AP is absolute pressure
// BP is the pressure,
// AL is the altitude,
// DA is the density altitude
// AD is air density
// RA is relative air density
float values[NUM_PARAMS];
float pstatic_Pa, temp_degC, windspeed_mps, winddir_deg, rel_humidity;
if (want_alive_msg)
IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
if (getValues(packet + MIN_PACKET_LENGTH, values)){
if (metric_input) {
//If the unit outputs are set for SI (or closest match), use these conversions
pstatic_Pa = values[BP] * 100.0; // original is hPa
temp_degC = values[TP]; // original is deg C
windspeed_mps = values[WS] * 0.2778; // original is km/h
winddir_deg = values[MG];
rel_humidity = values[RH];
} else {
//Otherwise use the default imperial units and convert to SI
pstatic_Pa = values[BP] * 3386.4; // original is inches Hg
temp_degC = (values[TP] - 32.0) * 5.0/9.0; // original is deg F
windspeed_mps = values[WS] * 0.44704; // original is miles per hour
winddir_deg = values[MG];
rel_humidity = values[RH];
}
// format has to match declaration in var/messages.xml
IvySendMsg("%d WEATHER %f %f %f %f %f\n",
ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg, rel_humidity);
}
}
/** The transmitter periodic function */
gboolean timeout_transmit_callback(gpointer data)
{
int i;
// Loop trough all the available rigidbodies (TODO: optimize)
for (i = 0; i < MAX_RIGIDBODIES; i++) {
// Check if ID's are correct
if (rigidBodies[i].id >= MAX_RIGIDBODIES) {
fprintf(stderr,
"Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n",
rigidBodies[i].id, MAX_RIGIDBODIES - 1);
exit(EXIT_FAILURE);
}
// Check if we want to transmit (follow) this rigid
if (aircrafts[rigidBodies[i].id].ac_id == 0) {
continue;
}
// When we don track anymore and timeout or start tracking
if (rigidBodies[i].nSamples < 1
&& aircrafts[rigidBodies[i].id].connected
&& (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) {
aircrafts[rigidBodies[i].id].connected = FALSE;
fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
} else if (rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
}
// Check if we still track the rigid
if (rigidBodies[i].nSamples < 1) {
continue;
}
// Update the last tracked
aircrafts[rigidBodies[i].id].connected = TRUE;
aircrafts[rigidBodies[i].id].lastSample = natnet_latency;
// Defines to make easy use of paparazzi math
struct EnuCoor_d pos, speed;
struct EcefCoor_d ecef_pos;
struct LlaCoor_d lla_pos;
struct DoubleQuat orient;
struct DoubleEulers orient_eulers;
// Add the Optitrack angle to the x and y positions
pos.x = cos(tracking_offset_angle) * rigidBodies[i].x - sin(tracking_offset_angle) * rigidBodies[i].y;
pos.y = sin(tracking_offset_angle) * rigidBodies[i].x + cos(tracking_offset_angle) * rigidBodies[i].y;
pos.z = rigidBodies[i].z;
// Convert the position to ecef and lla based on the Optitrack LTP
ecef_of_enu_point_d(&ecef_pos , &tracking_ltp , &pos);
lla_of_ecef_d(&lla_pos, &ecef_pos);
// Check if we have enough samples to estimate the velocity
rigidBodies[i].nVelocityTransmit++;
if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
// Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples)
double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) /
((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit);
rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time;
rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time;
rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time;
// Add the Optitrack angle to the x and y velocities
speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x - sin(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x + cos(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.z = rigidBodies[i].vel_z;
// Conver the speed to ecef based on the Optitrack LTP
ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel , &tracking_ltp , &speed);
}
// Copy the quaternions and convert to euler angles for the heading
orient.qi = rigidBodies[i].qw;
orient.qx = rigidBodies[i].qx;
orient.qy = rigidBodies[i].qy;
orient.qz = rigidBodies[i].qz;
double_eulers_of_quat(&orient_eulers, &orient);
// Calculate the heading by adding the Natnet offset angle and normalizing it
double heading = -orient_eulers.psi + 90.0 / 57.6 -
tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
NormRadAngle(heading);
printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id,
aircrafts[rigidBodies[i].id].ac_id
, rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading),
rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z);
/* Construct time of time of week (tow) */
time_t now;
time(&now);
struct tm *ts = localtime(&now);
uint32_t tow = (ts->tm_wday - 1)*(24*60*60*1000) + ts->tm_hour*(60*60*1000) + ts->tm_min*(60*1000) + ts->tm_sec*1000;
// Transmit the REMOTE_GPS packet on the ivy bus (either small or big)
if (small_packets) {
/* The local position is an int32 and the 11 LSBs of the (signed) x and y axis are compressed into
* a single integer. The z axis is considered unsigned and only the latter 10 LSBs are
* used.
*/
uint32_t pos_xyz = 0;
// check if position within limits
if (fabs(pos.x * 100.) < pow(2, 10)) {
pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm
} else {
fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x);
pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm
}
if (fabs(pos.y * 100.) < pow(2, 10)) {
pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm
} else {
fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y);
pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm
}
if (pos.z * 100. < pow(2, 10) && pos.z > 0.) {
pos_xyz |= (((uint32_t)(fabs(pos.z) * 100.0)) & 0x3FF); // bits 9-0 z position in cm
} else if (pos.z > 0.) {
fprintf(stderr, "Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z);
pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm
}
// printf("ENU Pos: %u (%.2f, %.2f, %.2f)\n", pos_xyz, pos.x, pos.y, pos.z);
/* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into
* a single integer.
*/
uint32_t speed_xyz = 0;
// check if speed within limits
if (fabs(speed.x * 100) < pow(2, 10)) {
speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
} else {
fprintf(stderr, "Warning!! X Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.x);
speed_xyz = (((uint32_t)(pow(2, 10) * speed.x / fabs(speed.x))) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
}
if (fabs(speed.y * 100) < pow(2, 10)) {
speed_xyz |= (((uint32_t)(speed.y * 100.0)) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
} else {
fprintf(stderr, "Warning!! Y Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.y);
speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
}
if (fabs(speed.z * 100) < pow(2, 9)) {
speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s
} else {
fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z);
speed_xyz |= (((uint32_t)(pow(2, 9) * speed.z / fabs(speed.z))) & 0x3FF); // bits 9-0 speed z in cm/s
}
/* The gps_small msg should always be less than 20 bytes including the pprz header of 6 bytes
* This is primarily due to the maximum packet size of the bluetooth msgs of 19 bytes
* increases the probability that a complete message will be accepted
*/
IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d %d",
(int16_t)(heading * 10000), // int16_t heading in rad*1e4 (2 bytes)
pos_xyz, // uint32 ENU X, Y and Z in CM (4 bytes)
speed_xyz, // uint32 ENU velocity X, Y, Z in cm/s (4 bytes)
tow, // uint32_t time of day
aircrafts[rigidBodies[i].id].ac_id); // uint8 rigid body ID (1 byte)
} else {
IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id,
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
(int)(ecef_pos.x * 100.0), //int32 ECEF X in CM
(int)(ecef_pos.y * 100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z * 100.0), //int32 ECEF Z in CM
(int)(DegOfRad(lla_pos.lat) * 10000000.0), //int32 LLA latitude in deg*1e7
(int)(DegOfRad(lla_pos.lon) * 10000000.0), //int32 LLA longitude in deg*1e7
(int)(lla_pos.alt * 1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z * 1000.0), //int32 HMSL height above mean sea level in mm
(int)(rigidBodies[i].ecef_vel.x * 100.0), //int32 ECEF velocity X in cm/s
(int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in cm/s
(int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in cm/s
tow,
(int)(heading * 10000000.0)); //int32 Course in rad*1e7
}
if (must_log) {
if (log_exists == 0) {
fp = fopen(nameOfLogfile, "w");
log_exists = 1;
}
if (fp == NULL) {
printf("I couldn't open file for writing.\n");
exit(0);
} else {
struct timeval cur_time;
gettimeofday(&cur_time, NULL);
fprintf(fp, "%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", aircrafts[rigidBodies[i].id].ac_id,
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
(int)(ecef_pos.x * 100.0), //int32 ECEF X in CM
(int)(ecef_pos.y * 100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z * 100.0), //int32 ECEF Z in CM
(int)(DegOfRad(lla_pos.lat) * 1e7), //int32 LLA latitude in deg*1e7
(int)(DegOfRad(lla_pos.lon) * 1e7), //int32 LLA longitude in deg*1e7
(int)(lla_pos.alt*1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z * 1000.0), //int32 HMSL height above mean sea level in mm
(int)(rigidBodies[i].ecef_vel.x * 100.0), //int32 ECEF velocity X in cm/s
(int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in cm/s
(int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in cm/s
(int)(heading * 10000000.0), //int32 Course in rad*1e7
(int)cur_time.tv_sec,
(int)cur_time.tv_usec);
}
}
// Reset the velocity differentiator if we calculated the velocity
if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
rigidBodies[i].vel_x = 0;
rigidBodies[i].vel_y = 0;
rigidBodies[i].vel_z = 0;
rigidBodies[i].nVelocitySamples = 0;
rigidBodies[i].totalVelocitySamples = 0;
rigidBodies[i].nVelocityTransmit = 0;
}
rigidBodies[i].nSamples = 0;
}
return TRUE;
}
void myMotifCallback(Widget w,XtPointer client_d,XtPointer call_d){
// sends the string on the bus
IvySendMsg (*((char**)client_d));
}
/* callback associated to "Hello" messages */
void HelloCallback (IvyClientPtr app, void *data, int argc, char **argv)
{
const char* arg = (argc < 1) ? "" : argv[0];
IvySendMsg ("Bonjour%s", arg);
}
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
//FIXME: electrical.vsupply is now a uint16
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, autopilot_flight_time);
autopilot_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;
autopilot_flight_time = 123;
#endif
printf("**** message received from iridium module ****\n");
printf("gps_lat %f\n", DegOfRad(gps_lat/1e7));
printf("gps_lon %f\n", DegOfRad(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("autopilot_flight_time %d\n", autopilot_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,
autopilot_flight_time);
}
else {
int i = 0;
// Raw print
printf("**** Raw message ****\n");
printf("Char: ");
for (i = 0; i < count; i++) printf("%c",buf[i]);
printf("\nHex: ");
for (i = 0; i < count; i++) printf("%x ",buf[i]);
printf("\n");
}
}
else {
printf("disconnect\n");
close(sock);
g_main_loop_quit(ml);
return 0;
}
return 1;
}
//Read tcp requests of connected clients
gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) {
GString *s = g_string_new(NULL);
GError *error = NULL;
GIOStatus ret = g_io_channel_read_line_string(source, s, NULL, &error);
if (ret == G_IO_STATUS_ERROR) {
//unref connection
GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL);
GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr));
//Read sender ip
if (verbose) {
printf("App Server: Communication error.. Removing client.. ->%s\n", g_inet_address_to_string(addr));
fflush(stdout);
}
//Remove client
remove_client(g_inet_address_to_string(addr));
//Free objects
g_string_free(s, TRUE);
g_object_unref(sockaddr);
g_object_unref(addr);
g_object_unref(data);
//Return false to stop listening this socket
return FALSE;
}
else{
//Read request command
gchar *RecString;
RecString=s->str;
//check client password
if ((strncmp(RecString, AppPass, strlen(AppPass))) == 0) {
//Password ok can send command
GString *incs = g_string_new(s->str);
incs = g_string_erase(s,0,(strlen(AppPass)+1));
IvySendMsg("%s",incs->str);
if (verbose) {
printf("App Server: Command passed to ivy: %s\n",incs->str);
fflush(stdout);
}
}
//AC data request. (Ignore client password)
else if ((strncmp(RecString, "getac ", strlen("getac "))) == 0) {
//AC data request
char AcData[BUFLEN];
//Read ac data
if (get_ac_data(RecString, AcData)) {
//Send requested data to client
GOutputStream * ostream = g_io_stream_get_output_stream (data);
g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error);
}
}
//Waypoint data request (Ignore client password)
else if ((strncmp(RecString, "getwp ", strlen("getwp "))) == 0) {
char AcData[BUFLEN];
//Read wp data of ac
if (get_wp_data(RecString, AcData)) {
//Send requested data to client
GOutputStream * ostream = g_io_stream_get_output_stream (data);
g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error);
}
}
//Waypoint data request (Ignore client password)
else if ((strncmp(RecString, "getbl ", strlen("getbl "))) == 0) {
char AcData[BUFLEN];
//Read block data of AC
if (get_bl_data(RecString, AcData)) {
//Send requested data to client
GOutputStream * ostream = g_io_stream_get_output_stream (data);
g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error);
}
}
//If client sends removeme command, remove it from broadcast list
else if ((strncmp( RecString, "removeme", strlen("removeme"))) == 0) {
GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL);
GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr));
//Read sender ip
if (verbose) {
printf("App Server: need to remove %s\n", g_inet_address_to_string(addr));
fflush(stdout);
}
//Remove client
remove_client(g_inet_address_to_string(addr));
//Free objects
g_string_free(s, TRUE);
g_object_unref(sockaddr);
g_object_unref(addr);
g_object_unref(data);
//Return false to stop listening this socket
return FALSE;
}
else {
//Unknown command
if (verbose) {
printf("App Server: Client send an unknown command or wrong password: (%s)\n",RecString);
fflush(stdout);
}
}
}
if (ret == G_IO_STATUS_EOF) {
//Client disconnected
if (verbose) {
GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL);
GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr));
printf("App Server: Client disconnected without saying 'bye':( ->%s\n", g_inet_address_to_string(addr));
remove_client(g_inet_address_to_string(addr));
fflush(stdout);
}
g_string_free(s, TRUE);
//Unref the socket and return false to allow the client to reconnect
g_object_unref(data);
return FALSE;
}
//None above.. Keep listening the socket
g_string_free(s, TRUE);
return TRUE;
}
/** The transmitter periodic function */
gboolean timeout_transmit_callback(gpointer data) {
int i;
// Loop trough all the available rigidbodies (TODO: optimize)
for(i = 0; i < MAX_RIGIDBODIES; i++) {
// Check if ID's are correct
if(rigidBodies[i].id >= MAX_RIGIDBODIES) {
fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1);
exit(EXIT_FAILURE);
}
// Check if we want to transmit (follow) this rigid
if(aircrafts[rigidBodies[i].id].ac_id == 0)
continue;
// When we don track anymore and timeout or start tracking
if(rigidBodies[i].nSamples < 1
&& aircrafts[rigidBodies[i].id].connected
&& (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) {
aircrafts[rigidBodies[i].id].connected = FALSE;
fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
}
else if(rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n",
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
}
// Check if we still track the rigid
if(rigidBodies[i].nSamples < 1)
continue;
// Update the last tracked
aircrafts[rigidBodies[i].id].connected = TRUE;
aircrafts[rigidBodies[i].id].lastSample = natnet_latency;
// Defines to make easy use of paparazzi math
struct EnuCoor_d pos, speed;
struct EcefCoor_d ecef_pos;
struct LlaCoor_d lla_pos;
struct DoubleQuat orient;
struct DoubleEulers orient_eulers;
// Add the Optitrack angle to the x and y positions
pos.x = cos(tracking_offset_angle) * rigidBodies[i].x + sin(tracking_offset_angle) * rigidBodies[i].y;
pos.y = sin(tracking_offset_angle) * rigidBodies[i].x - cos(tracking_offset_angle) * rigidBodies[i].y;
pos.z = rigidBodies[i].z;
// Convert the position to ecef and lla based on the Optitrack LTP
ecef_of_enu_point_d(&ecef_pos ,&tracking_ltp ,&pos);
lla_of_ecef_d(&lla_pos, &ecef_pos);
// Check if we have enough samples to estimate the velocity
rigidBodies[i].nVelocityTransmit++;
if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
// Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples)
double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) /
((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit);
rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time;
rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time;
rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time;
// Add the Optitrack angle to the x and y velocities
speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x + sin(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x - cos(tracking_offset_angle) * rigidBodies[i].vel_y;
speed.z = rigidBodies[i].vel_z;
// Conver the speed to ecef based on the Optitrack LTP
ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel ,&tracking_ltp ,&speed);
}
// Copy the quaternions and convert to euler angles for the heading
orient.qi = rigidBodies[i].qw;
orient.qx = rigidBodies[i].qx;
orient.qy = rigidBodies[i].qy;
orient.qz = rigidBodies[i].qz;
DOUBLE_EULERS_OF_QUAT(orient_eulers, orient);
// Calculate the heading by adding the Natnet offset angle and normalizing it
double heading = -orient_eulers.psi-tracking_offset_angle;
NormRadAngle(heading);
printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id
, rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading),
rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z);
// Transmit the REMOTE_GPS packet on the ivy bus
IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id,
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
(int)(ecef_pos.x*100.0), //int32 ECEF X in CM
(int)(ecef_pos.y*100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z*100.0), //int32 ECEF Z in CM
(int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7
(int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7
(int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm
(int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s
(int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s
(int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s
0,
(int)(heading*10000000.0)); //int32 Course in rad*1e7
// Reset the velocity differentiator if we calculated the velocity
if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
rigidBodies[i].vel_x = 0;
rigidBodies[i].vel_y = 0;
rigidBodies[i].vel_z = 0;
rigidBodies[i].nVelocitySamples = 0;
rigidBodies[i].totalVelocitySamples = 0;
rigidBodies[i].nVelocityTransmit = 0;
}
rigidBodies[i].nSamples = 0;
}
return TRUE;
}
void send_ivy(void)
{
float phi,theta,psi,z,zdot;
if (new_serial_data == 0)
return;
new_serial_data = 0;
phi = ((float) remote_uav.phi) / 1000.0f;
theta = ((float) remote_uav.theta) / 1000.0f;
psi = ((float) remote_uav.psi) / 1000.0f;
IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", remote_uav.ac_id);
IvySendMsg("%d ATTITUDE %f %f %f\n", remote_uav.ac_id, phi, psi, theta);
/*
remote_uav.utm_east = local_uav.utm_east;
remote_uav.utm_north = local_uav.utm_north + 5000;
remote_uav.utm_z = local_uav.utm_z + 1000;
remote_uav.utm_zone = local_uav.utm_zone;
remote_uav.speed = local_uav.speed * 4;
remote_uav.psi += 30.;
*/
/*
<message name="GPS" id="8">
<field name="mode" type="uint8" unit="byte_mask"/>
<field name="utm_east" type="int32" unit="cm" alt_unit="m"/>
<field name="utm_north" type="int32" unit="cm" alt_unit="m"/>
<field name="course" type="int16" unit="decideg" alt_unit="deg"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="speed" type="uint16" unit="cm/s" alt_unit="m/s"/>
<field name="climb" type="int16" unit="cm/s" alt_unit="m/s"/>
<field name="week" type="uint16" unit="weeks"/>
<field name="itow" type="uint32" unit="ms"/>
<field name="utm_zone" type="uint8"/>
<field name="gps_nb_err" type="uint8"/>
</message>
*/
IvySendMsg("%d GPS 3 %d %d 0 %d %d 0 0 0 %d 0\n", remote_uav.ac_id, remote_uav.utm_east, remote_uav.utm_north, remote_uav.utm_z, remote_uav.speed, remote_uav.utm_zone);
/*
<message name="FBW_STATUS" id="103">
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
<field name="frame_rate" type="uint8" unit="Hz"/>
<field name="mode" type="uint8" values="MANUAL|AUTO|FAILSAFE"/>
<field name="vsupply" type="uint8" unit="decivolt"/>
<field name="current" type="int32" unit="mA"/>
</message>
*/
IvySendMsg("%d FBW_STATUS 2 0 1 81 0 \n", remote_uav.ac_id);
z = ((float)remote_uav.utm_z) / 1000.0f;
zdot = remote_uav.climb;
IvySendMsg("%d ESTIMATOR %f %f \n", remote_uav.ac_id, z, zdot);
/*
<message name="DESIRED" id="16">
<field name="roll" type="float" format="%.2f" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
<field name="pitch" type="float" format="%.2f" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
<field name="course" type="float" format="%.1f" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
<field name="x" type="float" format="%.0f" unit="m"/>
<field name="y" type="float" format="%.0f" unit="m"/>
<field name="altitude" type="float" format="%.0f" unit="m"/>
<field name="climb" type="float" format="%.1f" unit="m/s"></field>
</message>
*/
IvySendMsg("%d DESIRED 0 0 0 0 0 %f 0 \n", remote_uav.ac_id, remote_uav.desired_alt);
/*
<message name="NAVIGATION" id="10">
<field name="cur_block" type="uint8"/>
<field name="cur_stage" type="uint8"/>
<field name="pos_x" type="float" unit="m" format="%.1f"/>
<field name="pos_y" type="float" unit="m" format="%.1f"/>
<field name="dist2_wp" type="float" format="%.1f" unit="m^2"/>
<field name="dist2_home" type="float" format="%.1f" unit="m^2"/>
<field name="circle_count" type="uint8"/>
<field name="oval_count" type="uint8"/>
</message>
*/
// IvySendMsg("%d NAVIGATION %d 0 0 0 0 0 0 0 \n", remote_uav.ac_id, remote_uav.block);
/*
<message name="BAT" id="12">
<field name="throttle" type="int16" unit="pprz"/>
<field name="voltage" type="uint8" unit="1e-1V" alt_unit="V" alt_unit_coef="0.1"/>
<field name="amps" type="int16" unit="A" alt_unit="A" />
<field name="flight_time" type="uint16" unit="s"/>
<field name="kill_auto_throttle" type="uint8" unit="bool"/>
<field name="block_time" type="uint16" unit="s"/>
<field name="stage_time" type="uint16" unit="s"/>
<field name="energy" type="int16" unit="mAh"/>
</message>
*/
// IvySendMsg("%d BAT 0 81 0 %ld 0 0 0 0\n", remote_uav.ac_id, count_serial);
/*
<message name="PPRZ_MODE" id="11">
<field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
<field name="ap_gaz" type="uint8" values="MANUAL|AUTO_THROTTLE|AUTO_CLIMB|AUTO_ALT"/>
<field name="ap_lateral" type="uint8" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
<field name="ap_horizontal" type="uint8" values="WAYPOINT|ROUTE|CIRCLE"/>
<field name="if_calib_mode" type="uint8" values="NONE|DOWN|UP"/>
<field name="mcu1_status" type="uint8" values="LOST|OK|REALLY_LOST"/>
</message>
*/
// IvySendMsg("%d PPRZ_MODE 0 0 0 0 0 0\n", remote_uav.ac_id);
/* // Needed to show any NAV info like current block number
<message name="NAVIGATION_REF" id="9">
<field name="utm_east" type="int32" unit="m"/>
<field name="utm_north" type="int32" unit="m"/>
<field name="utm_zone" type="uint8"/>
</message>
*/
static int delayer = 10;
delayer++;
if (delayer > 5)
{
// IvySendMsg("%d NAVIGATION_REF %d %d %d\n", remote_uav.ac_id, remote_uav.utm_east, remote_uav.utm_north, remote_uav.utm_zone);
delayer = 0;
}
count_serial++;
sprintf(status_serial_str, "Read %d from '%s': forwarding to IVY [%ld] {Rx=%ld} {Err=%ld}", remote_uav.ac_id, port, count_serial, rx_bytes, rx_error);
gtk_label_set_text( GTK_LABEL(status_serial), status_serial_str );
}
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);
/*
<message name="GPS" id="8">
<field name="mode" type="uint8" unit="byte_mask"/>
<field name="utm_east" type="int32" unit="cm" alt_unit="m"/>
<field name="utm_north" type="int32" unit="cm" alt_unit="m"/>
<field name="course" type="int16" unit="decideg" alt_unit="deg"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="speed" type="uint16" unit="cm/s" alt_unit="m/s"/>
<field name="climb" type="int16" unit="cm/s" alt_unit="m/s"/>
<field name="week" type="uint16" unit="weeks"/>
<field name="itow" type="uint32" unit="ms"/>
<field name="utm_zone" type="uint8"/>
<field name="gps_nb_err" type="uint8"/>
</message>
*/
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
/*
<message name="PPRZ_MODE" id="11">
<field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
<field name="ap_gaz" type="uint8" values="MANUAL|AUTO_THROTTLE|AUTO_CLIMB|AUTO_ALT"/>
<field name="ap_lateral" type="uint8" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
<field name="ap_horizontal" type="uint8" values="WAYPOINT|ROUTE|CIRCLE"/>
<field name="if_calib_mode" type="uint8" values="NONE|DOWN|UP"/>
<field name="mcu1_status" type="uint8" values="LOST|OK|REALLY_LOST"/>
</message>
*/
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
/*
<message name="AIRSPEED" id="54">
<field name="airspeed" type="float" unit="m/s"/>
<field name="airspeed_sp" type="float" unit="m/s"/>
<field name="airspeed_cnt" type="float" unit="m/s"/>
<field name="groundspeed_sp" type="float" unit="m/s"/>
</message>
*/
IvySendMsg("%d AIRSPEED %f %d %d %d",
AC_ID,
(float)(estimator_airspeed / 100.),
0, // airspeed_sp
0, // airspeed_cnt
0); // groundspeed_sp
/*
<message name="BAT" id="12">
<field name="throttle" type="int16" unit="pprz"/>
<field name="voltage" type="uint8" unit="1e-1V" alt_unit="V" alt_unit_coef="0.1"/>
<field name="amps" type="int16" unit="A" alt_unit="A" />
<field name="flight_time" type="uint16" unit="s"/>
<field name="kill_auto_throttle" type="uint8" unit="bool"/>
<field name="block_time" type="uint16" unit="s"/>
<field name="stage_time" type="uint16" unit="s"/>
<field name="energy" type="int16" unit="mAh"/>
</message>
*/
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);
/*
<message name="NAVIGATION" id="10">
<field name="cur_block" type="uint8"/>
<field name="cur_stage" type="uint8"/>
<field name="pos_x" type="float" unit="m" format="%.1f"/>
<field name="pos_y" type="float" unit="m" format="%.1f"/>
<field name="dist2_wp" type="float" format="%.1f" unit="m^2"/>
<field name="dist2_home" type="float" format="%.1f" unit="m^2"/>
<field name="circle_count" type="uint8"/>
<field name="oval_count" type="uint8"/>
</message>
*/
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
/*
<message name="ESTIMATOR" id="42">
<field name="z" type="float" unit="m"/>
<field name="z_dot" type="float" unit="m/s"/>
</message>
*/
IvySendMsg("%d ESTIMATOR %f %d",
AC_ID,
gps_alt / 1000.,
0); // z_dot
/*
<message name="ATTITUDE" id="6">
<field name="phi" type="float" unit="rad" alt_unit="deg"/>
<field name="psi" type="float" unit="rad" alt_unit="deg"/>
<field name="theta" type="float" unit="rad" alt_unit="deg"/>
</message>
*/
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;
}
int main(int argc, char **argv) {
int i;
//Get process id
ProcessID= getpid();
// default password
AppPass = defaultAppPass;
// try environment variable first, set to default if failed
IvyBus = getenv("IVYBUS");
if (IvyBus == NULL) IvyBus = defaultIvyBus;
// Parse options
for (i = 1; i < argc; ++i) {
if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) {
print_help();
exit(0);
}
else if (strcmp(argv[i], "-t") == 0) {
tcp_port = atoi(argv[++i]);
}
else if (strcmp(argv[i], "-u") == 0) {
udp_port = atoi(argv[++i]);
}
else if (strcmp(argv[i], "-b") == 0) {
IvyBus = argv[++i];
}
else if (strcmp(argv[i], "-p") == 0) {
AppPass = argv[++i];
}
else if (strcmp(argv[i], "-v") == 0) {
verbose = 1;
}
else if (strcmp(argv[i], "-utcp") == 0) {
uTCP = 1;
}
else {
printf("App Server: Unknown option\n");
print_help();
exit(0);
}
}
if (verbose) {
printf("### Paparazzi App Server ###\n");
printf("Server listen port (TCP) : %d\n", tcp_port);
if (uTCP) {
printf("Server using TCP communication..\n");
}else{
printf("Server broadcast port (UDP) : %d\n", udp_port);
}
printf("Control Pass : %s\n", AppPass);
printf("Ivy Bus : %s\n", IvyBus);
fflush(stdout);
}
//Create tcp listener
#if !GLIB_CHECK_VERSION (2, 35, 1)
// init GLib type system (only for older version)
g_type_init();
#endif
GSocketService *service = g_socket_service_new();
GInetAddress *address = g_inet_address_new_any(G_SOCKET_FAMILY_IPV6); //G_SOCKET_FAMILY_IPV4 could be used
GSocketAddress *socket_address = g_inet_socket_address_new(address, tcp_port);
//Add listener
g_socket_listener_add_address(G_SOCKET_LISTENER(service), socket_address, G_SOCKET_TYPE_STREAM,
G_SOCKET_PROTOCOL_TCP, NULL, NULL, NULL);
g_object_unref(socket_address);
g_object_unref(address);
g_socket_service_start(service);
//Connect listening signal
g_signal_connect(service, "incoming", G_CALLBACK(new_connection), NULL);
//Here comes the ivy bindings
IvyInit ("PPRZ_App_Server", "Papparazzi App Server Ready!", NULL, NULL, NULL, NULL);
IvyBindMsg(Ivy_All_Msgs, NULL, "(^ground (\\S*) (\\S*) .*)");
IvyBindMsg(on_app_server_NEW_AC, NULL, "ground NEW_AIRCRAFT (\\S*)");
IvyBindMsg(on_app_server_GET_CONFIG, NULL, "(\\S*) ground CONFIG (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_app_server_AIRCRAFTS, NULL, "(\\S*) ground AIRCRAFTS (\\S*)");
IvyStart(IvyBus);
GMainLoop *loop = g_main_loop_new(NULL, FALSE);
if (verbose) {
printf("Starting App Server\n");
fflush(stdout);
}
IvySendMsg("app_server");
g_timeout_add(100, request_ac_list, NULL);
g_main_loop_run(loop);
if (verbose) {
printf("Stoping App Server\n");
fflush(stdout);
}
return 0;
}
