christoph/failnix
1
Fork 0
Code Releases Activity

Add wasm tacle-bench targets

Browse Source
This commit is contained in:
Christoph Urlacher
2026-06-12 20:06:22 +02:00
parent 30daa8a00c
commit 08c2e9c13d
1122 changed files with 520422 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/downlink.c Normal file
View File

@ -0,0 +1,65 @@
/*
downlink.c
Copyright (C) 2003 Antoine Drouin
This file is part of paparazzi.
paparazzi 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.
paparazzi 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 <string.h>
#include "downlink.h"
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
#define READ_GFLOAT_FROM_BUF(dest, buf) dest = *((gfloat*)(buf)); buf += 4;
#define READ_GUINT32_FROM_BUF(dest, buf) dest = GUINT32_FROM_LE(*((guint32*)(buf))); buf += 4;
#define READ_GUINT16_FROM_BUF(dest, buf) dest = GUINT16_FROM_LE(*((guint16*)(buf))); buf += 2;
#define READ_GINT32_FROM_BUF(dest, buf) dest = GINT32_FROM_LE(*((gint32*)(buf))); buf += 4;
#define READ_GINT16_FROM_BUF(dest, buf) dest = GINT16_FROM_LE(*((gint16*)(buf))); buf += 2;
#define READ_GUINT8_FROM_BUF(dest, buf) dest = (guint8)(*(buf++));
struct Transport *downlink_new( struct PprzProtocol *protocol,
void( *err_callback )( gpointer callback_data, GError *error ),
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
gpointer callback_data )
{
struct Transport *transport;
guint *fixed_payload_len = g_new( guint,
g_list_length( protocol->msgs_classes ) );
guint i = 0;
GList *msgs_classes = protocol->msgs_classes;
while ( msgs_classes ) {
fixed_payload_len[ i ] = ( ( struct PprzMsgClass * )msgs_classes->data )->size;
// printf("fixed_payload_len : %d %d\n", i, fixed_payload_len[ i ]);
i++;
msgs_classes = msgs_classes->next;
}
transport = transport_new( TRUE, /* fixed_size */
TRUE, /* two bytes checksum */
i, /* nb_msg */
fixed_payload_len, /* size_msg */
100, /* max_msg_size */
DL_STX, DL_ETX, /* STX, ETX */
err_callback, /* err callback */
msg_callback, /* msg callback */
callback_data ); /* callback data */
return transport;
}

451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c Normal file
View File

@ -0,0 +1,451 @@
/*
geometry.c
Copyright (C) 2003 Antoine Drouin
This file is part of paparazzi.
paparazzi 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.
paparazzi 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 "geometry.h"
//#include <math.h>
#include "math_papabench.h"
const struct ellipsoid ellipsoids[ ELLIPSOID_NB ] = {
{-168., -60., 320., 6378249.2, 0.0034075495234250643, 0.08248325676}, /* NTF */
{ 0., 0., 0., 6378137.0, 0.0033528106647474805, 0.08181919106}, /* WGS84 */
{-87.0, -98., -121.0, 6378388.0, 0.003367003367003367, 0.08199188998}, /* ED50 */
{ 0.0, 125.0, 194.0, 6378067.6, 0.0033155460257, 0.08181919106} /* NAD27 */
};
/* From http://www.tandt.be/wis/WiS/eqntf.html and http://www.ign.fr/MP/GEOD/geodesie/coordonnees.html */
const struct lambert lamberts[ LAMBERT_NB ] = {
{ /* LAMBERTI */
&ellipsoids[ NTF ],
RAD_OF_DEG( DECIMAL( 49., 30., 0. ) ), /* phi0 */
0.991996665, /* lphi0 */
5457616.674, /* r0 */
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
600000, /* x0 */
200000, /* y0 */
5657617, /* ys */
0.99987734 /* k0 */
},
{ /* LAMBERTII */
&ellipsoids[ NTF ],
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
0.921557361, /* lphi0 */
5999695.77, /* r0 */
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
600000, /* x0 */
2200000, /* y0 */
6199696, /* ys */
0.99987742 /* k0 */
},
{ /* LAMBERTIIE */
&ellipsoids[ NTF ],
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
0.921557361, /* lphi0 */
5999695.77, /* r0 */
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
600000, /* x0 */
2200000, /* y0 */
8199696, /* ys */
0.99987742 /* k0 */
},
{ /* LAMBERTIII */
&ellipsoids[ NTF ],
RAD_OF_DEG( DECIMAL( 44., 6., 0. ) ), /* phi0 */
0.921557361, /* lphi0 */
6591905.08, /* r0 */
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
600000, /* x0 */
3200000, /* y0 */
6791905, /* ys */
0.99987750 /* k0 */
},
{ /* LAMBERTIV */
&ellipsoids[ NTF ],
RAD_OF_DEG( DECIMAL( 42., 9., 54. ) ), /* phi0 */
0.921557361, /* lphi0 */
7053300.18, /* r0 */
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
234, /* x0 */ /* FIXME mmmmmmm suspicious */
4185861, /* y0 */
7239162, /* ys */
0.99994471 /* k0 */
},
};
double distance_c2d( const struct double_c2d a, const struct double_c2d b )
{
return sqrt( ( a.x - b.x ) * ( a.x - b.x ) + ( a.y - b.y ) * ( a.y - b.y ) );
}
struct double_c2d vector_c2d( const struct double_c2d a,
const struct double_c2d b )
{
struct double_c2d v;
v.x = b.x - a.x;
v.y = b.y - a.y;
return v;
}
struct double_c2d unit_vector_c2d( const struct double_c2d a,
const struct double_c2d b )
{
struct double_c2d v = vector_c2d( a, b );
scale_c2d_s( &v, 1 / module_c2d( v ) );
return v;
}
struct double_c2d normal_vector_c2d( const struct double_c2d u )
{
struct double_c2d v;
v.x = -u.y;
v.y = u.x;
return v;
}
double vect_prod_c2d( const struct double_c2d a, const struct double_c2d b )
{
return a.x * b.y - a.y * b.x;
}
double scal_prod_c2d( const struct double_c2d a, const struct double_c2d b )
{
return a.x * b.x + a.y * b.y;
}
void rotate_c2d( struct double_c2d *a, double alpha )
{
double tmp;
tmp = a->x * cos( alpha ) - a->y * sin( alpha );
a->y = a->x * sin( alpha ) + a->y * cos( alpha );
a->x = tmp;
}
double norme2_c2d( const struct double_c2d a )
{
return a.x * a.x + a.y * a.y;
}
double module_c2d( const struct double_c2d a )
{
return sqrt( norme2_c2d( a ) );
}
void translate_c2d( struct double_c2d *a, const struct double_c2d v )
{
add_c2d_s( a, v );
}
void add_c2d_s( struct double_c2d *a, const struct double_c2d b )
{
a->x = a->x + b.x;
a->y = a->y + b.y;
}
struct double_c2d add_c2d( struct double_c2d a, const struct double_c2d b )
{
add_c2d_s( &a, b );
return a;
}
void normalise_c2d_s( struct double_c2d *a )
{
scale_c2d_s( a, 1 / module_c2d( *a ) );
}
struct double_c2d normalise_c2d( struct double_c2d a )
{
normalise_c2d_s( &a );
return a;
}
void minus_c2d_s( struct double_c2d *a, const struct double_c2d b )
{
a->x = a->x - b.x;
a->y = a->y - b.y;
}
struct double_c2d minus_c2d( struct double_c2d a, const struct double_c2d b )
{
minus_c2d_s( &a, b );
return a;
}
void scale_c2d_s( struct double_c2d *v, double factor )
{
v->x = v->x * factor;
v->y = v->y * factor;
}
struct double_c2d scale_c2d( struct double_c2d v, double factor )
{
scale_c2d_s( &v, factor );
return v;
}
#define MPI_PI(val)\
{\
while (val> M_PI) val -= 2. * M_PI;\
while (val<-M_PI) val += 2. * M_PI;\
}
double mpi_pi( double val )
{
MPI_PI( val );
return val;
};
struct double_c2d lin_interp_c2d( struct double_c2d a, double ta,
struct double_c2d b, double tb, double t )
{
double delta_t = tb - ta;
struct double_c2d res;
res.x = a.x + ( t - ta ) / delta_t *( b.x - a.x );
res.y = a.y + ( t - ta ) / delta_t *( b.y - a.y );
return res;
}
double wind_dir_from_angle_deg( double deg )
{
double w = 180. + 90. - deg;
while ( w > 360 ) w -= 360;
return w;
}
double wind_dir_from_angle_rad( double rad )
{
double w = M_PI + M_PI_2 - rad;
while ( w > 2 * M_PI ) w -= ( 2 * M_PI );
return w;
}
double heading_of_to_angle_deg( double angle )
{
double a = 90. - angle + 360.;
while ( a >= 180. ) a -= 360.;
return a;
}
double heading_of_to_angle_rad( double angle )
{
double two_pi = 2 * M_PI;
double a = M_PI_2 - angle + two_pi;
while ( a >= two_pi ) a -= two_pi;
return a;
}
#define G 9.81
#define ACCEL_DOWN 0
double compute_phi( struct double_c2d curspeed, struct double_c2d last_speed,
double delta_t )
{
double accel_norm, accel_tgt, phi;
struct double_c2d accel = minus_c2d( curspeed, last_speed );
// printf("%lf %lf %lf %lf %lf\n", curspeed.x, curspeed.y, last_speed.x, last_speed.y, delta_t);
scale_c2d_s( &accel, 1 / delta_t );
accel_norm = vect_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
accel_tgt = scal_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
phi = -atan2( accel_norm, G - ACCEL_DOWN );
return phi;
}
double angle_c2d( struct double_c2d u, struct double_c2d v )
{
double alpha1 = atan2( u.y, u.x );
double alpha2 = atan2( v.y, v.x );
double alpha = alpha2 - alpha1;
MPI_PI( alpha );
return alpha;
}
struct double_p2d p2d_of_c2d( const struct double_c2d c )
{
struct double_p2d p;
p.r = module_c2d( c );
p.teta = atan2( c.y, c.x );
return p;
}
struct double_c2d c2d_of_p2d( const struct double_p2d p )
{
struct double_c2d c;
c.x = p.r * cos( p.teta );
c.y = p.r * sin( p.teta );
return c;
}
struct double_p2d add_p2d( const struct double_p2d p1,
const struct double_p2d p2 )
{
/* p.r = sqrt(p1.r*p1.r + p2.r*p2.r + 2 * p1.r * p2.r * cos ( p1.teta - p2.teta )); */
/* p.teta = 0; */
struct double_c2d c1 = c2d_of_p2d( p1 );
struct double_c2d c2 = c2d_of_p2d( p2 );
struct double_c2d c3 = add_c2d( c1, c2 );
struct double_p2d p = p2d_of_c2d( c3 );
return p;
}
struct double_p2d minus_p2d( const struct double_p2d p1,
const struct double_p2d p2 )
{
struct double_c2d c1 = c2d_of_p2d( p1 );
struct double_c2d c2 = c2d_of_p2d( p2 );
struct double_c2d c3 = minus_c2d( c1, c2 );
struct double_p2d p = p2d_of_c2d( c3 );
return p;
}
struct double_c2d c2d_of_polar( double r, double teta )
{
struct double_c2d res = {r * cos( teta ), r * sin( teta )};
return res;
}
void translate_c3d( struct double_c3d *a, const struct double_c3d v )
{
a->x = a->x + v.x;
a->y = a->y + v.y;
a->z = a->z + v.z;
}
void scale_c3d( struct double_c3d *v, double factor )
{
v->x = v->x * factor;
v->y = v->y * factor;
v->z = v->z * factor;
}
static double inverse_latitude_isometrique( double lat, double e,
double epsilon )
{
double exp_l = exp( lat );
double phi0 = 2. * atan( exp_l - M_PI_2 );
double loop( double phi ) {
double sin_phi = e * sin( phi );
double new_phi = 2. * atan ( pow( ( 1. + sin_phi ) / ( 1. - sin_phi ),
e / 2 ) * exp_l ) - M_PI_2;
if ( fabs( phi - new_phi ) < epsilon ) return new_phi;
return loop( new_phi );
}
return loop( phi0 );
}
static double lambert_n ( const enum type_lambert lmb )
{
return sin( lamberts[ lmb ].phi0 );
}
static double lambert_c ( const enum type_lambert lmb )
{
return lamberts[ lmb ].r0 * exp( lamberts[ lmb ].phi0 * lambert_n( lmb ) );
}
void lambert_of_e2d ( const enum type_lambert lmb,
const enum type_ellipsoid elps,
struct double_e2d e2d_pos, struct double_c2d *lmb_pos )
{
double n, ll, r, gamma;
e2d_of_e2d( NTF, elps, &e2d_pos );
n = lambert_n( lmb );
ll = LATITUDE_ISOMETRIQUE( e2d_pos.lat, lamberts[ lmb ].ellipsoid->e );
r = lamberts[ lmb ].r0 * exp( lamberts[ lmb ].lphi0 * sin( lamberts[ lmb ].phi0 ) ) *
exp( -ll * n );
gamma = ( e2d_pos.lon - lamberts[ lmb ].lambda0 ) * n;
lmb_pos->x = ( double )lamberts[ lmb ].x0 + r * sin( gamma );
lmb_pos->y = ( double )lamberts[ lmb ].ys - r * cos( gamma );
}
void e2d_of_lambert ( const enum type_lambert lmb,
const enum type_ellipsoid elps,
struct double_c2d c_pos, struct double_e2d *e_pos )
{
double dx = c_pos.x - lamberts[ lmb ].x0;
double dy = c_pos.y - lamberts[ lmb ].ys;
double r = sqrt( dx * dx + dy * dy );
double gamma = atan2( dx, -dy );
double lambda = lamberts[ lmb ].lambda0 + gamma / lambert_n( lmb );
double ll = -1. / lambert_n( lmb ) * log( fabs( r / lambert_c( lmb ) ) );
double phi = inverse_latitude_isometrique( ll, lamberts[ lmb ].ellipsoid->e,
1e-11 );
e_pos->lon = lambda;
e_pos->lat = phi;
e2d_of_e2d( elps, NTF, e_pos );
}
void e2d_of_e2d( const enum type_ellipsoid dest, const enum type_ellipsoid src,
struct double_e2d *e2d_pos )
{
if ( dest != src ) {
double sin_lat = sin( e2d_pos->lat );
double cos_lat = cos( e2d_pos->lat );
double sin_lon = sin( e2d_pos->lon );
double cos_lon = cos( e2d_pos->lon );
double d16 = E_SQUARE( ellipsoids[ src ].df );
double d17 = E_SQUARE( ellipsoids[ dest ].df );
double d18 = ellipsoids[ src ].a / sqrt ( 1.0 - d16 * sin_lat * sin_lat );
double d20 = d18 * cos_lat * cos_lon;
double d21 = d18 * cos_lat * sin_lon;
double d22 = d18 * ( 1.0 - d16 ) * sin_lat;
double d23 = d20 - ellipsoids[ dest ].dx + ellipsoids[ src ].dx;
double d24 = d21 - ellipsoids[ dest ].dy + ellipsoids[ src ].dy;
double d25 = d22 - ellipsoids[ dest ].dz + ellipsoids[ src ].dz;
double d26 = sqrt ( d23 * d23 + d24 * d24 );
double d27 = atan2( d25, d26 * ( 1.0 - ellipsoids[ dest ].df ) );
double d28 = ellipsoids[ dest ].a * ( 1.0 - ellipsoids[ dest ].df );
double d29 = E_PRIME_SQUARE( ellipsoids[ dest ].df );
double d3 = atan2( ( d25 + d29 * d28 * sin( d27 ) * sin( d27 ) * sin( d27 ) ),
( d26 - d17 * ellipsoids[ dest ].a * cos( d27 ) * cos( d27 ) * cos( d27 ) ) );
double d4 = atan2( d24, d23 );
e2d_pos->lon = d4;
e2d_pos->lat = d3;
}
}
/*
converts 3d cartesian coordinate to 3d ellipsoidal
*/
void e3d_of_c3d( const enum type_ellipsoid elps, struct double_c3d c_pos,
struct double_e3d *e_pos )
{
#define WGS_84_SMA 6378137.00
#define WGS_84_SMB 6356752.31
#define E2 ((WGS_84_SMA*WGS_84_SMA-WGS_84_SMB*WGS_84_SMB)/(WGS_84_SMA*WGS_84_SMA))
#define PREC 0.000000000001
double dist = sqrt( c_pos.x * c_pos.x + c_pos.y * c_pos.y );
double v, old_phi, new_phi = atan2( ( double )c_pos.z, dist * ( 1 - E2 ) );
do {
old_phi = new_phi;
v = WGS_84_SMA / sqrt( 1 - ( E2 * sin( old_phi ) * sin( old_phi ) ) );
new_phi = atan2( ( ( double )c_pos.z + E2 * v * sin ( old_phi ) ), dist );
} while ( fabs( new_phi - old_phi ) > PREC );
e_pos->lat = new_phi;
e_pos->lon = atan2( c_pos.y, c_pos.x );
e_pos->h = ( dist / cos ( new_phi ) ) - v;
}

193
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/logger.c Normal file
View File

@ -0,0 +1,193 @@
/*
logger.c
Copyright (C) 2003 Antoine Drouin
This file is part of paparazzi.
paparazzi 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.
paparazzi 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 <sys/types.h>
#include <dirent.h>
#include <libgen.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <sys/stat.h>
#include <glib.h>
#include "logger.h"
//static GList* logger_parse_messages(struct PprzProtocol* protocol, xmlDocPtr doc, xmlNodePtr cur);
static GList *logger_parse_message( struct PprzProtocol *protocol, GList *msgs,
const guchar *buf );
struct Logger *logger_new( const gchar *out_file, xmlDocPtr doc )
{
const gchar *opening = "<data>\n";
struct Logger *this = g_new( struct Logger, 1 );
if ( this ) {
GError *err = NULL;
gchar *copy = strdup( out_file );
gchar *out_dir = dirname( copy );
DIR *d;
if ( !( d = opendir( out_dir ) ) ) {
if ( errno == ENOENT ) {
if ( mkdir( out_dir, S_IRWXU | S_IRWXG ) ) {
g_message( "log directory %s doesn't exist and could not create it", out_dir );
/* FIXME : FREE */
return NULL;
}
} else {
g_message( "could not acces log directory %s %s", out_dir,
g_strerror( errno ) );
/* FIXME : FREE */
return NULL;
}
}
closedir( d );
g_free( copy );
xmlSaveFormatFile ( out_file, doc, 1 );
/* FIXME : check result */
this->channel = g_io_channel_new_file( out_file, "a", &err );
if ( err ) {
g_message( "could not open log file %s : %s", out_file, err->message );
g_error_free( err );
g_free( this );
return NULL;
}
g_io_channel_write_chars( this->channel, opening, strlen( opening ), NULL,
NULL );
}
return this;
}
void logger_free( struct Logger *this )
{
const gchar *closing = "</data>\n</log>\n";
g_io_channel_write_chars( this->channel, closing, strlen( closing ), NULL,
NULL );
g_io_channel_flush( this->channel, NULL );
// g_io_channel_free(this->channel);
g_free( this );
}
void logger_log( struct Logger *this, struct PprzMsg *msg )
{
GString *str;
pprz_protocol_ascii_of_msg( msg, &str );
if ( str ) {
GError *err = NULL;
guint len;
g_io_channel_write_chars ( this->channel, str->str, str->len, &len, &err );
if ( err ) {
g_warning( "log write failed %s", err->message );
g_error_free( err );
g_string_free( str, TRUE );
return;
}
g_string_free( str, TRUE );
g_io_channel_flush( this->channel, &err );
if ( err ) {
g_warning( "log flush failed %s", err->message );
g_error_free( err );
return;
}
}
}
GList *logger_parse_log( const gchar *filename,
struct PprzProtocol **protocol )
{
xmlDocPtr doc;
xmlNodePtr cur;
GList *msgs = NULL;
const gchar *protocol_closure = "</protocol>";
gchar *contents, *data_start;
gsize len;
GError *err;
gboolean res = g_file_get_contents( filename, &contents, &len, &err );
if ( !res ) {
g_message( "could not read file %s : %s", filename, err->message );
g_error_free( err );
return NULL;
}
if ( !( data_start = g_strstr_len( contents, len, protocol_closure ) ) ) {
g_free( contents );
g_message( "protocol closing not found" );
return NULL;
}
data_start += strlen( protocol_closure );
if ( !( doc = xmlParseMemory( contents, data_start - contents ) ) ) {
g_free( contents );
g_message( "error parsing protocol xml" );
return NULL;
}
if ( !( cur = xmlDocGetRootElement( doc ) ) ) {
g_free( contents );
g_message( "empty protocol" );
return NULL;
}
if ( !( *protocol = pprz_protocol_new_from_xml( doc, cur ) ) ) {
g_free( contents );
g_message( "error interpreting protocol" );
return NULL;
}
msgs = logger_parse_message( *protocol, msgs, data_start );
g_free( contents );
return msgs;
}
/* static GList* logger_parse_messages(struct PprzProtocol* protocol, xmlDocPtr doc, xmlNodePtr cur) { */
/* if (!cur) return NULL; */
/* if (xmlStrcmp(cur->name, (const xmlChar *) "data")) */
/* return cur->next?logger_parse_messages(protocol, doc, cur->next):NULL; */
/* cur = cur->xmlChildrenNode; */
/* if (xmlStrcmp(cur->name, (const xmlChar *) "text")) */
/* return NULL; */
/* return logger_parse_message(protocol, NULL, cur->content); */
/* } */
static GList *logger_parse_message( struct PprzProtocol *protocol, GList *msgs,
const guchar *buf )
{
guchar *next;
struct PprzMsg *msg = pprz_protocol_msg_new_of_ascii( protocol, buf );
if ( msg )
msgs = g_list_append( msgs, msg );
if ( ( next = index( buf, '\n' ) ) )
return logger_parse_message( protocol, msgs, next + 1 );
return msgs;
}

93
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/math.c Normal file
View File

@ -0,0 +1,93 @@
/*
Math library emulation functions
Copyright (C) 2007 IRIT - UPS <casse@irit.fr>
This file is part of papabench.
paparazzi 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.
paparazzi 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 papabench; see the file COPYING. If not, write to
the Free Software Foundation, 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.
*/
#include <math.h>
double pp_atan2( double x, double y )
{
double coeff_1 = M_PI / 4;
double coeff_2 = 3 * coeff_1;
double abs_y = fabs( y ) + 1e-10;
double angle, r;
if ( x > 0 ) {
r = ( x - abs_y ) / ( x + abs_y );
angle = coeff_1 - coeff_1 * r;
} else {
r = ( x + abs_y ) / ( abs_y - x );
angle = coeff_2 - coeff_1 * r;
}
if ( y < 0 )
return ( -angle );
else
return angle;
}
/* Calculates sin(x), angle x must be in rad.
Range: -pi/2 <= x <= pi/2
Precision: +/- .000,000,005
*/
double pp_sin( double x )
{
double xi, y, q, q2;
int sign;
xi = x;
sign = 1;
_Pragma( "loopbound min 0 max 0" )
while ( xi < -1.57079632679489661923 ) xi += 6.28318530717958647692;
_Pragma( "loopbound min 0 max 0" )
while ( xi > 4.71238898038468985769 ) xi -= 6.28318530717958647692;
if ( xi > 1.57079632679489661923 ) {
xi -= 3.141592653589793238462643;
sign = -1;
}
q = xi / 1.57079632679;
q2 = q * q;
y = ( ( ( ( .00015148419 * q2
- .00467376557 ) * q2
+ .07968967928 ) * q2
- .64596371106 ) * q2
+ 1.57079631847 ) * q;
return ( sign < 0 ? -y : y );
}
#define SQRT_PRECISION 5
double pp_sqrt( double n )
{
#if 0
float x, m;
int i, e;
/* compute the approximation */
m = frexpf( n, &e );
x = ldexp( m, e / 2 );
/* perform the computation */
_Pragma( "loopbound min 5 max 5 )
for ( i = 0; i < SQRT_PRECISION; i++ )
x = ( x + n / x ) / 2;
return x;
#endif
}

192
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/network.c Normal file
View File

@ -0,0 +1,192 @@
/*
$id:$
Copyright (C) 2003 Antoine Drouin
This file is part of paparazzi.
paparazzi 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.
paparazzi 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.
*/
/*
network functions
*/
#include "network.h"
#include <string.h>
#include <errno.h>
#include <netdb.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
static gboolean server_on_data_received( GIOChannel *source,
GIOCondition condition, gpointer data );
static gboolean client_on_data_received( GIOChannel *source,
GIOCondition condition, gpointer data );
struct NetworkServer *network_server_new( struct PprzProtocol *protocol,
const char *bcast_addr, const int bcast_port,
GFunc rcv_callback, GFunc err_callback, gpointer user_data )
{
struct NetworkServer *this = g_new( struct NetworkServer, 1 );
if ( this ) {
GIOChannel *channel;
struct protoent *pte = getprotobyname( "UDP" );
static int so_broadcast = TRUE;
this->bcast_socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto );
setsockopt( this->bcast_socket, SOL_SOCKET, SO_BROADCAST, &so_broadcast,
sizeof( so_broadcast ) );
this->bcast_addr.sin_family = PF_INET;
this->bcast_addr.sin_port = htons( bcast_port );
this->bcast_addr.sin_addr.s_addr = inet_addr( bcast_addr );
channel = g_io_channel_unix_new( this->bcast_socket );
g_io_add_watch ( channel, G_IO_IN | G_IO_PRI, server_on_data_received, this );
this->protocol = protocol;
this->rcv_callback = rcv_callback;
this->err_callback = err_callback;
this->user_data = user_data;
}
return this;
}
#define FIXME_LEN 2048
void network_server_dispatch( struct NetworkServer *this, struct PprzMsg *msg )
{
guchar buf[ FIXME_LEN ];
GTimeVal *date = ( GTimeVal * )buf;
guchar *type = buf + sizeof( GTimeVal );
guchar *data = type + 1;
guint len = sizeof( GTimeVal ) + msg->class->size;
*date = msg->date;
*type = msg->class->id;
memcpy( data, msg->bytes, msg->class->size - 1 );
if ( sendto( this->bcast_socket, buf, len, 0,
( struct sockaddr * )&this->bcast_addr, sizeof( this->bcast_addr ) ) == -1 )
g_message( "error broadcasting" );
}
struct NetworkClient *network_client_new( struct PprzProtocol *protocol,
const char *bcast_addr, const int bcast_port,
GFunc rcv_callback, GFunc err_callback, gpointer user_data )
{
struct NetworkClient *this = g_new( struct NetworkClient, 1 );
if ( this ) {
int so_reuseaddr = TRUE;
GIOChannel *channel;
struct protoent *pte = getprotobyname( "UDP" );
this->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto );
setsockopt( this->socket, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr,
sizeof( so_reuseaddr ) );
this->bcast_addr.sin_family = PF_INET;
this->bcast_addr.sin_port = htons( bcast_port );
this->bcast_addr.sin_addr.s_addr = inet_addr( bcast_addr );
bind( this->socket, ( struct sockaddr * )&this->bcast_addr,
sizeof( this->bcast_addr ) );
channel = g_io_channel_unix_new( this->socket );
g_io_add_watch ( channel, G_IO_IN | G_IO_PRI, client_on_data_received, this );
this->protocol = protocol;
this->rcv_callback = rcv_callback;
this->err_callback = err_callback;
this->user_data = user_data;
}
return this;
}
void network_client_send_to_server( struct NetworkClient *this,
struct PprzMsg *msg )
{
guchar buf[ FIXME_LEN ];
GTimeVal *date = ( GTimeVal * )buf;
guchar *type = buf + sizeof( GTimeVal );
guchar *data = type + 1;
guint len = sizeof( GTimeVal ) + msg->class->size;
*date = msg->date;
*type = msg->class->id;
memcpy( data, msg->bytes, msg->class->size - 1 );
if ( sendto( this->socket, buf, len, 0,
( struct sockaddr * )&this->bcast_addr, sizeof( this->bcast_addr ) ) == -1 )
g_message( "error sending to server" );
}
static gboolean server_on_data_received( GIOChannel *source,
GIOCondition condition, gpointer data )
{
guchar buf[ FIXME_LEN ];
struct sockaddr_in from;
int from_len;
gint len;
struct NetworkServer *this = ( struct NetworkServer * )data;
if ( ( len = recvfrom( this->bcast_socket, buf, FIXME_LEN, 0,
( struct sockaddr * )&from, &from_len ) ) == -1 ) {
this->err_callback( NULL, this->user_data );
return FALSE;
} else {
struct PprzMsg *msg;
GTimeVal *date = ( GTimeVal * )buf;
guchar *ptr_type = buf + sizeof( GTimeVal );
if ( !( msg = pprz_protocol_msg_new_of_bin( this->protocol, ptr_type,
*date ) ) ) {
this->err_callback( NULL, this->user_data );
return TRUE;
}
this->rcv_callback( msg, this->user_data );
// g_message("network data received %s %d", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
}
return TRUE;
}
static gboolean client_on_data_received( GIOChannel *source,
GIOCondition condition, gpointer data )
{
guchar buf[ FIXME_LEN ];
struct sockaddr_in from;
int from_len;
gint len;
struct NetworkClient *this = ( struct NetworkClient * )data;
if ( ( len = recvfrom( this->socket, buf, FIXME_LEN, 0,
( struct sockaddr * )&from, &from_len ) ) == -1 ) {
g_message( "client_on_data_received [ %s ]", strerror( errno ) );
this->err_callback( NULL, this->user_data );
return TRUE;
} else {
struct PprzMsg *msg;
GTimeVal *date = ( GTimeVal * )buf;
guchar *ptr_type = buf + sizeof( GTimeVal );
if ( !( msg = pprz_protocol_msg_new_of_bin( this->protocol, ptr_type,
*date ) ) ) {
this->err_callback( NULL, this->user_data );
return TRUE;
}
this->rcv_callback( msg, this->user_data );
// g_message("network data received %s %d", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
}
this->server_addr = from;
return TRUE;
}

551
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/protocol.c Normal file
View File

@ -0,0 +1,551 @@
/*
$Id: protocol.c,v 1.1 2011-01-18 12:48:45 moellmer Exp $
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
This file is part of paparazzi.
paparazzi 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.
paparazzi 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 "protocol.h"
#include <stdlib.h>
#include <string.h>
struct PprzTypInfo {
gchar *xml_name;
guint size;
gchar *default_format;
};
const struct PprzTypInfo type_info[ TYP_NB ] = {
{"uint8", 1, "%hhu"},
{"uint16", 2, "%hu" },
{"uint32", 4, "%u" },
{"int8", 1, "%hhd"},
{"int16", 2, "%hd" },
{"int32", 4, "%d" },
{"float", 4, "%f" },
{"uint8_array", 1, "%0x"}
};
static gboolean parse_xml_msg( struct PprzProtocol *this, xmlNodePtr ptr );
static gboolean parse_xml_field( struct PprzMsgClass *msg_class,
xmlNodePtr ptr );
static void ascii_of_fields( struct PprzMsg *msg, GList *fields_class,
GString *buf );
static gboolean field_of_ascii( GList *fields_class, gchar *line,
struct PprzMsg *msg );
static enum PprzFieldType type_of_string( const gchar *type_str );
static struct PprzMsgClass *msg_class_of_name( struct PprzProtocol *this,
const gchar *name );
static struct PprzMsgClass *msg_class_of_id( struct PprzProtocol *this,
guchar id );
static struct PprzFieldClass *field_class_of_name( struct PprzMsg *msg,
const gchar *name );
/* static struct PprzFieldClass* field_class_of_id(struct PprzMsg* msg, guchar id); */
struct PprzProtocol *pprz_protocol_new_from_xml( xmlDocPtr doc,
xmlNodePtr cur )
{
struct PprzProtocol *this;
if ( !cur ) return NULL; /* empty document */
if ( xmlStrcmp( cur->name,
( const xmlChar * ) "protocol" ) ) /* not protocol */
return cur->next ? pprz_protocol_new_from_xml( doc, cur->next ) : NULL;
this = g_new( struct PprzProtocol, 1 );
this->msgs_classes = NULL;
this->msgs_classes_by_name = g_hash_table_new( g_str_hash, g_str_equal );
if ( !parse_xml_msg( this, cur->xmlChildrenNode ) ) {
g_free( this );
return NULL;
}
this->nb_msgs_classes = g_list_length( this->msgs_classes );
this->msgs_classes_by_id = g_new( struct PprzMsgClass *,
this->nb_msgs_classes );
{
GList *cur = this->msgs_classes;
guint i = 0;
while ( cur ) {
this->msgs_classes_by_id[ i ] = cur->data;
i++;
cur = cur->next;
}
}
return this;
}
const gchar *pprz_protocol_str_of_field_type( enum PprzFieldType type )
{
return type_info[ type ].xml_name;
}
const guint pprz_protocol_size_of_field_type( enum PprzFieldType type )
{
return type_info[ type ].size;
}
static enum PprzFieldType type_of_string( const gchar *type_str )
{
guint ret = 0;
while ( ret < TYP_NB && strcmp( type_info[ ret ].xml_name, type_str ) )
ret++;
return ret;
}
struct PprzMsg *pprz_message_new( struct PprzMsgClass *msg_class,
GTimeVal date )
{
struct PprzMsg *this = g_new( struct PprzMsg, 1 );
this->date = date;
this->class = msg_class;
this->bytes = g_new( guchar, this->class->size - 1 );
return this;
}
struct PprzMsg *pprz_protocol_msg_new_by_id( struct PprzProtocol *this,
guchar id, GTimeVal date )
{
struct PprzMsgClass *msg_class = msg_class_of_id( this, id );
if ( msg_class ) return pprz_message_new( msg_class, date );
return NULL;
}
struct PprzMsg *pprz_protocol_msg_new_by_name( struct PprzProtocol *this,
const gchar *name, GTimeVal date )
{
struct PprzMsgClass *msg_class = msg_class_of_name( this, name );
if ( msg_class ) return pprz_message_new( msg_class, date );
return NULL;
}
struct PprzMsg *pprz_protocol_msg_new_of_bin( struct PprzProtocol *this,
const guchar *buf, GTimeVal date )
{
struct PprzMsg *msg = pprz_protocol_msg_new_by_id( this, buf[ 0 ], date );
if ( msg )
memcpy( msg->bytes, buf + 1, msg->class->size - 1 );
return msg;
}
struct PprzMsg *pprz_protocol_msg_new_of_ascii( struct PprzProtocol *this,
const gchar *line )
{
gchar *kw;
GTimeVal date;
struct PprzMsg *msg;
if ( sscanf( line, "%ld.%06ld %as", &date.tv_sec, &date.tv_usec, &kw ) != 3 )
return NULL;
if ( !( msg = pprz_protocol_msg_new_by_name( this, kw, date ) ) )
return NULL;
if ( msg->class->fields_classes ) { // message has arguments
gchar *space = index( line, ' ' );
g_assert( space );
g_assert( space + 1 ); // checked before by sscanf
space = index( space + 1, ' ' );
g_assert( space ); // checked before by sscanf
if ( !field_of_ascii( msg->class->fields_classes, space, msg ) ) {
pprz_msg_free( msg );
return NULL;
}
}
return msg;
}
void pprz_msg_free( struct PprzMsg *this )
{
g_free( this->bytes );
g_free( this );
}
gboolean pprz_msg_set_field( struct PprzMsg *msg,
struct PprzFieldClass *field_class, gconstpointer value )
{
/*#ifdef WITH_SWITCH
switch (field_class->type) {
case TYP_UINT_8:
(guint8*)(msg->bytes+field_class->offset) = *(guint8*)value;
break;
case TYP_UINT_16:
(guint16*)(msg->bytes+field_class->offset) = *(guint16*)value;
break;
case TYP_UINT_32:
(guint32*)(msg->bytes+field_class->offset) = *(guint32*)value;
break;
case TYP_INT_8:
(gint8*)(msg->bytes+field_class->offset) = *(gint8*)value;
break;
case TYP_INT_16:
(gint16*)(msg->bytes+field_class->offset) = *(gint16*)value;
break;
case TYP_INT_32:
(gint32*)(msg->bytes+field_class->offset) = *(gint32*)value;
break;
case TYP_FLOAT:
(gfloat*)(msg->bytes+field_class->offset) = *(gfloat*)value;
break;
case TYP_ARRAY_UINT_8:
// g_string_append_printf(buf, format, *((gfloat*)data));
break;
default:
g_warning("in ascii of field : unknown type");
}
#else*/
if ( field_class->type == TYP_UINT_8 ) {
*( guint8 * )( msg->bytes + field_class->offset ) = *( guint8 * )value;
break;
} else
if ( field_class->type == TYP_UINT_16 ) {
*( guint16 * )( msg->bytes + field_class->offset ) = *( guint16 * )value;
break;
} else
if ( field_class->type == TYP_UINT_32 ) {
*( guint32 * )( msg->bytes + field_class->offset ) = *( guint32 * )value;
break;
} else
if ( field_class->type == TYP_INT_8 ) {
*( gint8 * )( msg->bytes + field_class->offset ) = *( gint8 * )value;
break;
} else
if ( field_class->type == TYP_INT_16 ) {
*( gint16 * )( msg->bytes + field_class->offset ) = *( gint16 * )value;
break;
} else
if ( field_class->type == TYP_INT_32 ) {
*( gint32 * )( msg->bytes + field_class->offset ) = *( gint32 * )value;
break;
} else
if ( field_class->type == TYP_FLOAT ) {
*( gfloat * )( msg->bytes + field_class->offset ) = *( gfloat * )value;
break;
} else
if ( field_class->type == TYP_ARRAY_UINT_8 ) {
// g_string_append_printf(buf, format, *((gfloat*)data));
break;
} else
g_warning( "in ascii of field : unknown type" );
//#endif
return TRUE;
}
gboolean pprz_msg_set_field_by_name( struct PprzMsg *msg, const gchar *name,
gconstpointer value )
{
struct PprzFieldClass *field_class = field_class_of_name( msg, name );
if ( field_class )
return pprz_msg_set_field( msg, field_class, value );
return FALSE;
}
gboolean pprz_msg_set_field_by_id( struct PprzMsg *msg, guint id,
gconstpointer value )
{
GList *cell = g_list_nth( msg->class->fields_classes, id );
if ( cell )
return pprz_msg_set_field( msg, cell->data, value );
return FALSE;
}
gpointer pprz_msg_get_field( struct PprzMsg *msg,
struct PprzFieldClass *field_class )
{
return msg->bytes + field_class->offset;
}
gpointer pprz_msg_get_field_by_name( struct PprzMsg *msg, const gchar *name )
{
struct PprzFieldClass *field_class = field_class_of_name( msg, name );
if ( field_class )
return pprz_msg_get_field( msg, field_class );
g_message( "pprz_msg_get_field_by_name : unknown field [ %s ] in msg [ %s ]", name,
msg->class->name );
return NULL;
}
#define DEFAULT_LINE_LEN 256
void pprz_protocol_ascii_of_msg( struct PprzMsg *msg, GString **buf )
{
*buf = g_string_sized_new( DEFAULT_LINE_LEN );
g_string_printf( *buf, "%ld.%06ld %s ", msg->date.tv_sec, msg->date.tv_usec,
msg->class->name );
if ( msg->class->fields_classes ) ascii_of_fields( msg,
msg->class->fields_classes, *buf );
g_string_append_printf( *buf, "\n" );
}
void pprz_msg_ascii_of_field( struct PprzMsg *msg,
struct PprzFieldClass *field_class, GString *buf )
{
guchar *format;
guchar *data;
format = field_class->format;
if ( !format ) format = type_info[ field_class->type ].default_format;
data = msg->bytes + field_class->offset;
/*#ifdef WITH_SWITCH
switch (field_class->type) {
case TYP_UINT_8:
g_string_append_printf(buf, format, *((guint8*)data));
break;
case TYP_UINT_16:
g_string_append_printf(buf, format, *((guint16*)data));
break;
case TYP_UINT_32:
g_string_append_printf(buf, format, *((guint32*)data));
break;
case TYP_INT_8:
g_string_append_printf(buf, format, *((gint8*)data));
break;
case TYP_INT_16:
g_string_append_printf(buf, format, *((gint16*)data));
break;
case TYP_INT_32:
g_string_append_printf(buf, format, *((gint32*)data));
break;
case TYP_FLOAT:
g_string_append_printf(buf, format, *((gfloat*)data));
break;
case TYP_ARRAY_UINT_8:
// g_string_append_printf(buf, format, *((gfloat*)data));
break;
default:
g_warning("in ascii of field : unknown type");
}
#else*/
if ( field_class->type == TYP_UINT_8 ) {
g_string_append_printf( buf, format, *( ( guint8 * )data ) );
break;
} else
if ( field_class->type == TYP_UINT_16 ) {
g_string_append_printf( buf, format, *( ( guint16 * )data ) );
break;
} else
if ( field_class->type == TYP_UINT_32 ) {
g_string_append_printf( buf, format, *( ( guint32 * )data ) );
break;
} else
if ( field_class->type == TYP_INT_8 ) {
g_string_append_printf( buf, format, *( ( gint8 * )data ) );
break;
} else
if ( field_class->type == TYP_INT_16 ) {
g_string_append_printf( buf, format, *( ( gint16 * )data ) );
break;
} else
if ( field_class->type == TYP_INT_32 ) {
g_string_append_printf( buf, format, *( ( gint32 * )data ) );
break;
} else
if ( field_class->type == TYP_FLOAT ) {
g_string_append_printf( buf, format, *( ( gfloat * )data ) );
break;
} else
if ( field_class->type == TYP_ARRAY_UINT_8 ) {
// g_string_append_printf(buf, format, *((gfloat*)data));
break;
} else
g_warning( "in ascii of field : unknown type" );
//#endif
}
static gboolean field_of_ascii( GList *fields_classes, gchar *line,
struct PprzMsg *msg )
{
struct PprzFieldClass *field_class = ( struct PprzFieldClass * )
fields_classes->data;
gchar *end_ptr;
gchar *ptr_field = msg->bytes + field_class->offset;
/*#ifdef WITH_SWITCH
switch (field_class->type) {
case TYP_UINT_8:
((guint8*)ptr_field) = (guint8)strtoul(line, &end_ptr, 10);
break;
case TYP_UINT_16:
((guint16*)ptr_field) = (guint16)strtoul(line, &end_ptr, 10);
break;
case TYP_UINT_32:
((guint32*)ptr_field) = (guint32)strtoul(line, &end_ptr, 10);
break;
case TYP_INT_8:
((gint8*)ptr_field) = (gint8)strtol(line, &end_ptr, 10);
break;
case TYP_INT_16:
((gint16*)ptr_field) = (gint16)strtol(line, &end_ptr, 10);
break;
case TYP_INT_32:
((gint32*)ptr_field) = (gint32)strtol(line, &end_ptr, 10);
break;
case TYP_FLOAT:
((gfloat*)ptr_field) = (gfloat)strtod(line, &end_ptr);
break;
case TYP_ARRAY_UINT_8:
break;
default:
printf("in field of ascii: unknown type\n");
}
#else*/
if ( field_class->type == TYP_UINT_8 ) {
*( ( guint8 * )ptr_field ) = ( guint8 )strtoul( line, &end_ptr, 10 );
break;
} else
if ( field_class->type == TYP_UINT_16 ) {
*( ( guint16 * )ptr_field ) = ( guint16 )strtoul( line, &end_ptr, 10 );
break;
} else
if ( field_class->type == TYP_UINT_32 ) {
*( ( guint32 * )ptr_field ) = ( guint32 )strtoul( line, &end_ptr, 10 );
break;
} else
if ( field_class->type == TYP_INT_8 ) {
*( ( gint8 * )ptr_field ) = ( gint8 )strtol( line, &end_ptr, 10 );
break;
} else
if ( field_class->type == TYP_INT_16 ) {
*( ( gint16 * )ptr_field ) = ( gint16 )strtol( line, &end_ptr, 10 );
break;
} else
if ( field_class->type == TYP_INT_32 ) {
*( ( gint32 * )ptr_field ) = ( gint32 )strtol( line, &end_ptr, 10 );
break;
} else
if ( field_class->type == TYP_FLOAT ) {
*( ( gfloat * )ptr_field ) = ( gfloat )strtod( line, &end_ptr );
break;
} else
if ( field_class->type == TYP_ARRAY_UINT_8 )
break;
else
printf( "in field of ascii: unknown type\n" );
//#endif
if ( fields_classes->next )
return field_of_ascii( fields_classes->next, end_ptr, msg );
return TRUE;
}
static void ascii_of_fields( struct PprzMsg *msg, GList *fields_class,
GString *buf )
{
struct PprzFieldClass *field_class = ( struct PprzFieldClass * )
fields_class->data;
pprz_msg_ascii_of_field( msg, field_class, buf );
if ( fields_class->next ) {
g_string_append_printf( buf, " " );
ascii_of_fields( msg, fields_class->next, buf );
}
}
static gboolean parse_xml_msg( struct PprzProtocol *this, xmlNodePtr ptr )
{
struct PprzMsgClass *new_msg_class;
if ( !xmlNodeIsText ( ptr ) ) {
if ( xmlStrcmp( ptr->name, ( const xmlChar * )"message" ) ) /* not a message */
return FALSE;
new_msg_class = g_new( struct PprzMsgClass, 1 );
new_msg_class->id = g_list_length( this->msgs_classes );
new_msg_class->name = xmlGetProp( ptr, "id" );
new_msg_class->fields_classes = NULL;
new_msg_class->fields_classes_by_name = g_hash_table_new( g_str_hash,
g_str_equal );
new_msg_class->size = 1;
this->msgs_classes = g_list_append( this->msgs_classes, new_msg_class );
g_hash_table_insert( this->msgs_classes_by_name, new_msg_class->name,
new_msg_class );
if ( ptr->xmlChildrenNode &&
!parse_xml_field( new_msg_class, ptr->xmlChildrenNode ) )
/* fixme : free space */
return FALSE;
new_msg_class->nb_fields_classes = g_list_length(
new_msg_class->fields_classes );
new_msg_class->fields_classes_by_id = g_new( struct PprzFieldClass *,
new_msg_class->nb_fields_classes );
{
GList *cur = new_msg_class->fields_classes;
guint i = 0;
while ( cur ) {
new_msg_class->fields_classes_by_id[ i ] = cur->data;
i++;
cur = cur->next;
}
}
}
if ( ptr->next )
return parse_xml_msg( this, ptr->next );
return TRUE;
}
static gboolean parse_xml_field( struct PprzMsgClass *msg_class,
xmlNodePtr ptr )
{
struct PprzFieldClass *new_field_class;
xmlChar *type_str;
if ( !xmlNodeIsText ( ptr ) ) {
if ( xmlStrcmp( ptr->name, ( const xmlChar * )"field" ) ) /* not a field */
return FALSE;
new_field_class = g_new( struct PprzFieldClass, 1 );
new_field_class->name = xmlGetProp( ptr, "id" );
new_field_class->format = xmlGetProp( ptr, "format" );
new_field_class->unit = xmlGetProp( ptr, "unit" );
new_field_class->description = xmlGetProp( ptr, "description" );
type_str = xmlGetProp( ptr, "type" );
if ( ( new_field_class->type = type_of_string( type_str ) ) >= TYP_NB ) {
/* fixme : free space */
return FALSE;
}
xmlFree( type_str );
msg_class->fields_classes = g_list_append( msg_class->fields_classes,
new_field_class );
g_hash_table_insert( msg_class->fields_classes_by_name, new_field_class->name,
new_field_class );
new_field_class->offset = msg_class->size - 1;
msg_class->size += type_info[ new_field_class->type ].size;
}
if ( ptr->next )
/* check return and free space */
return parse_xml_field( msg_class, ptr->next );
return TRUE;
}
static struct PprzMsgClass *msg_class_of_name( struct PprzProtocol *this,
const gchar *name )
{
return g_hash_table_lookup( this->msgs_classes_by_name, name );
}
static struct PprzMsgClass *msg_class_of_id( struct PprzProtocol *this,
guchar id )
{
if ( id >= this->nb_msgs_classes )
return NULL;
return this->msgs_classes_by_id[ id ];
}
static struct PprzFieldClass *field_class_of_name( struct PprzMsg *msg,
const gchar *name )
{
return g_hash_table_lookup( msg->class->fields_classes_by_name, name );
}
/* static struct PprzFieldClass* field_class_of_id(struct PprzMsg* msg, guchar id) { */
/* GList* cell = g_list_nth(msg->class->fields_classes, id); */
/* return cell?cell->data:NULL; */
/* } */

52
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/traces.c Normal file
View File

@ -0,0 +1,52 @@
/*
traces.c
Copyright (C) 2003 Antoine Drouin
This file is part of paparazzi.
paparazzi 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.
paparazzi 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 "traces.h"
#ifdef TRACE_ON
unsigned long TraceLevel = TRACE_ERROR | TRACE_TRANSPORT
; // | TRACE_MODEM_READ ;// | TRACE_DATA_READ | TRACE_DOWNLINK; //| TRACE_MODEM_READ;// ;//| TRACE_MSG// | TRACE_DOWNLINK;
// unsigned long TraceLevel = TRACE_DOWNLINK | TRACE_MSG | TRACE_ERROR ;
const char *print_hex( const unsigned char *buf, unsigned int len )
{
const char d2h[ ] = "0123456789ABCDEF";
int i = 0;
#define HEX_BUF_LEN 1024
#define FMT_LEN 3
#define FMT "%c%c "
static unsigned char priv_buf[ HEX_BUF_LEN ];
/* printf("[ %d ] ", len); */
for ( i = 0; i < len && i < HEX_BUF_LEN / FMT_LEN; i++ ) {
// printf(FMT, d2h[ (int)buf[ i ]/16 ], d2h[ (int)buf[ i ]%16 ]);
sprintf( priv_buf + FMT_LEN * i, FMT, d2h[ ( int )buf[ i ] / 16 ],
d2h[ ( int )buf[ i ] % 16 ] );
}
if ( len == 0 )
priv_buf[ 0 ] = '\0';
return priv_buf;
}
#endif

244
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/transport.c Normal file
View File

@ -0,0 +1,244 @@
#include "transport.h"
#include "geometry.h"
#include "traces.h"
static void parse_buf( struct Transport *this );
static gboolean check_checksum( struct Transport *this, guint msg_len );
static void remove_n_from_buf( struct Transport *this, guint len );
static void remove_until_stx( struct Transport *this );
struct Transport *transport_new( gboolean fixed_size,
gboolean two_bytes_checksum, guint nb_msg,
guint *size_msg, guint max_msg_size,
guchar stx, guchar etx,
void( *err_callback )( gpointer callback_data, GError *error ),
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
gpointer callback_data )
{
struct Transport *this = g_new( struct Transport, 1 );
if ( this ) {
this->stx = stx;
this->etx = etx;
this->err_callback = err_callback;
this->msg_callback = msg_callback;
this->callback_data = callback_data;
this->buf_len = 0;
this->fixed_size = fixed_size;
this->two_bytes_checksum = two_bytes_checksum;
this->quark = g_quark_from_string( "Transport" );
g_get_current_time( &this->start_date );
if ( fixed_size ) {
this->nb_msg_type = nb_msg;
this->size_msg = size_msg;
//this->max_msg_size = FIXME find max size in array;;
} else
this->max_msg_size = max_msg_size;
this->nb_byte_last_status = 0;
this->nb_msg_last_status = 0;
this->status.nb_byte = 0;
this->status.nb_msg = 0;
this->status.nb_err = 0;
}
return this;
}
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
struct TransportStatus *transport_get_status( struct Transport *this )
{
GTimeVal now;
gdouble duration;
g_get_current_time( &now );
this->status.run_time = DELAY_SEC_OF_TIMEVAL( now, this->start_date );
duration = GDOUBLE_FROM_DIF_TV( now, this->last_status_date );
this->status.byte_rate = this->nb_byte_last_status / duration;
this->status.msg_rate = this->nb_msg_last_status / duration;
this->nb_byte_last_status = 0;
this->nb_msg_last_status = 0;
this->last_status_date = now;
return &this->status;
}
void transport_free( struct Transport *this )
{
g_free( this );
}
void transport_feed_data( struct Transport *this, const guchar *buf,
guint len )
{
if ( len >= TRANSPORT_BUF_LEN - this->buf_len ) {
GError *err = g_error_new( this->quark,
TRANSPORT_BUF_OVFW, /* guint code */
"buffer overflow" /* const gchar *format */
);
this->err_callback( this->callback_data, err );
g_error_free( err );
} else {
this->status.nb_byte += len;
this->nb_byte_last_status += len;
memcpy( this->buf + this->buf_len, buf, len );
this->buf_len += len;
parse_buf( this );
}
}
static gboolean check_checksum( struct Transport *this, guint msg_len )
{
guint checksum_idx = msg_len - TRANSPORT_TAIL_LEN;
guint i;
if ( this->two_bytes_checksum ) {
guchar cka = 0, ckb = 0;
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ ) {
cka += this->buf[ i ];
ckb += cka;
}
return ( cka == this->buf[ checksum_idx ] &&
ckb == this->buf[ checksum_idx + 1 ] );
} else {
guchar checksum = 0;
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ )
checksum ^= this->buf[ i ];
if ( checksum != this->buf[ checksum_idx ] )
TRACE( TRACE_ERROR, "transport checksum error (found : 0x%02X read : 0x%02X)\n",
checksum, this->buf[ checksum_idx ] );
return checksum == this->buf[ checksum_idx ];
}
}
static void remove_n_from_buf( struct Transport *this, guint len )
{
memmove( this->buf, this->buf + len, this->buf_len - len );
this->buf_len -= len;
}
static void remove_until_stx( struct Transport *this )
{
guchar *stx = memchr( this->buf, this->stx, this->buf_len );
if ( stx ) {
if ( stx != this->buf ) {
memmove( this->buf, stx, this->buf_len - ( stx - this->buf ) );
this->buf_len -= ( stx - this->buf );
}
} else
this->buf_len = 0;
}
static gboolean get_msg_len( struct Transport *this, guint *len )
{
if ( this->fixed_size ) {
guint type = this->buf[ TRANSPORT_PAYLOAD_OFFSET ];
if ( type >= this->nb_msg_type ) { /* BAP ID */
TRACE( TRACE_TRANSPORT, "%d bad type\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
this->buf_len ) );
this->status.nb_err++;
{
GError *err = g_error_new( this->quark,
TRANSPORT_INVALID_MSG_ID, /* guint code */
"invalid msg id" /* const gchar *format */
);
this->err_callback( this->callback_data, err );
g_error_free( err );
}
return FALSE;
} else *len = this->size_msg[ type ] + TRANSPORT_HEAD_LEN + TRANSPORT_TAIL_LEN;
} else {
*len = this->buf[ TRANSPORT_PAYLOAD_OFFSET ] + TRANSPORT_HEAD_LEN +
TRANSPORT_TAIL_LEN;
if ( *len >= this->max_msg_size ) { /* BAD LEN */
TRACE( TRACE_TRANSPORT, "%d bad len\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
this->status.nb_err++;
{
GError *err = g_error_new( this->quark,
TRANSPORT_INVALID_MSG_LEN, /* guint code */
"invalid message len" /* const gchar *format */
);
this->err_callback( this->callback_data, err );
g_error_free( err );
}
return FALSE;
}
}
return TRUE;
}
static void parse_buf( struct Transport *this )
{
/* make sure first byte in buffer is STX */
remove_until_stx( this );
// if (this->fixed_size)
// TRACE(TRACE_TRANSPORT, "parse_buf %s\n", print_hex(this->buf, this->buf_len));
if ( this->buf_len < TRANSPORT_PAYLOAD_OFFSET ) /* NO PAYLOAD */
TRACE( TRACE_TRANSPORT_VERB, "no payload\n" );
else {
guint msg_len;
if ( !get_msg_len( this,
&msg_len ) ) { /* BAD LEN ( or TYPE) */
remove_n_from_buf( this, 1 );
parse_buf( this );
} else {
if ( this->buf_len < msg_len ) /* NOT ENOUGTH DATA */
TRACE( TRACE_TRANSPORT_VERB, "not enough data\n" );
else {
if ( !check_checksum( this, msg_len ) ) {
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
this->buf_len ) );
this->status.nb_err++;
{
GError *err = g_error_new( this->quark,
TRANSPORT_CHECKSUM_ERROR, /* guint code */
"checksum error" /* const gchar *format */
);
this->err_callback( this->callback_data, err );
g_error_free( err );
}
remove_n_from_buf( this, 1 );
parse_buf( this );
} else {
if ( !this->two_bytes_checksum && this->buf[ msg_len - 1 ] != this->etx ) {
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
this->buf_len ) );
TRACE( TRACE_TRANSPORT_VERB, "%d is not ETX\n", this->buf[ msg_len - 1 ] );
this->status.nb_err++;
{
GError *err = g_error_new( this->quark,
TRANSPORT_NO_ETX, /* guint code */
"NO ETX" /* const gchar *format */
);
this->err_callback( this->callback_data, err );
g_error_free( err );
}
remove_n_from_buf( this, 1 );
parse_buf( this );
} else {
struct TransportMsg msg;
guint payload_len = msg_len - TRANSPORT_HEAD_LEN - TRANSPORT_TAIL_LEN;
g_get_current_time( &msg.date );
msg.len = payload_len;
msg.data = this->buf + TRANSPORT_PAYLOAD_OFFSET;
this->status.nb_msg++;
this->nb_msg_last_status++;
TRACE( TRACE_TRANSPORT_VERB,
"transport msg date : %ld%06ld len : %d data : %s\n", msg.date.tv_sec,
msg.date.tv_usec, msg.len, print_hex( msg.data, msg.len ) );
this->msg_callback( this->callback_data, &msg );
remove_n_from_buf( this, msg_len );
parse_buf( this );
}
}
}
}
}
}

12
targets/wasm-tacle/parallel/PapaBench/sw/lib/crt0/powerpc-elf-crt0.c Normal file
View File

@ -0,0 +1,12 @@
int main( int argc, char **argv, char **envp );
static char *argv[ ] = { "", 0 };
static char *envp[ ] = { 0 };
void _start( void )
{
__asm( "lis 1," STACK ">>16" );
__asm( "addis 1,1," STACK "&0xffff" );
main( 1, argv, envp );
while ( 1 );
}