Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: switch to big-endian metric packets internally 

now that all the sim code is internal to the APM git repo, we can
choose saner packet formats
This commit is contained in:
Andrew Tridgell 2011-12-02 22:12:58 +11:00
parent 3a377255f0
commit a0baf71322
5 changed files with 27 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Tools/autotest/jsbsim/runsim.py
View File

@ -50,7 +50,7 @@ def setup_home(home):
def process_sitl_input(buf): def process_sitl_input(buf):
'''process control changes from SITL sim''' '''process control changes from SITL sim'''
(aileron, elevator, rudder, throttle) = struct.unpack('>dddd', buf) (aileron, elevator, rudder, throttle) = struct.unpack('<4d', buf)
if aileron != sitl_state.aileron: if aileron != sitl_state.aileron:
jsb_set('fcs/aileron-cmd-norm', aileron) jsb_set('fcs/aileron-cmd-norm', aileron)
sitl_state.aileron = aileron sitl_state.aileron = aileron
@ -78,24 +78,24 @@ def process_jsb_input(buf):
if e.errno not in [ 111 ]: if e.errno not in [ 111 ]:
raise raise
simbuf = struct.pack('>ddddddddddddddddI', simbuf = struct.pack('<16dI',
fdm.get('latitude', units='degrees'), fdm.get('latitude', units='degrees'),
fdm.get('longitude', units='degrees'), fdm.get('longitude', units='degrees'),
fdm.get('altitude', units='feet'), fdm.get('altitude', units='meters'),
fdm.get('psi', units='degrees'), fdm.get('psi', units='degrees'),
fdm.get('v_north', units='fps'), fdm.get('v_north', units='mps'),
fdm.get('v_east', units='fps'), fdm.get('v_east', units='mps'),
fdm.get('A_X_pilot', units='fpss'), fdm.get('A_X_pilot', units='mpss'),
fdm.get('A_Y_pilot', units='fpss'), fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='fpss'), fdm.get('A_Z_pilot', units='mpss'),
fdm.get('phidot', units='dps'), fdm.get('phidot', units='dps'),
fdm.get('thetadot', units='dps'), fdm.get('thetadot', units='dps'),
fdm.get('psidot', units='dps'), fdm.get('psidot', units='dps'),
fdm.get('phi', units='degrees'), fdm.get('phi', units='degrees'),
fdm.get('theta', units='degrees'), fdm.get('theta', units='degrees'),
fdm.get('psi', units='degrees'), fdm.get('psi', units='degrees'),
fdm.get('vcas', units='knots'), fdm.get('vcas', units='mps'),
0x4c56414d) 0x4c56414e)
try: try:
sim_out.send(simbuf) sim_out.send(simbuf)
except socket.error as e: except socket.error as e:

14
Tools/autotest/pysim/sim_quad.py
View File

@ -38,14 +38,14 @@ def sim_send(m, a, r):
if not e.errno in [ errno.ECONNREFUSED ]: if not e.errno in [ errno.ECONNREFUSED ]:
raise raise
buf = struct.pack('>ddddddddddddddddI', buf = struct.pack('<16dI',
a.latitude, a.longitude, util.m2ft(a.altitude), a.yaw, a.latitude, a.longitude, a.altitude, a.yaw,
util.m2ft(a.velocity.x), util.m2ft(a.velocity.y), a.velocity.x, a.velocity.y,
util.m2ft(a.accelerometer.x), util.m2ft(a.accelerometer.y), util.m2ft(a.accelerometer.z), a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
a.roll_rate, a.pitch_rate, a.yaw_rate, a.roll_rate, a.pitch_rate, a.yaw_rate,
a.roll, a.pitch, a.yaw, a.roll, a.pitch, a.yaw,
util.m2ft(math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y)), math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
0x4c56414d) 0x4c56414e)
try: try:
sim_out.send(buf) sim_out.send(buf)
except socket.error as e: except socket.error as e:
@ -70,7 +70,7 @@ def sim_recv(m, a, r):
if len(buf) != 32: if len(buf) != 32:
return return
(m0, m1, m2, m3, (m0, m1, m2, m3,
r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7]) = struct.unpack('>ffffHHHHHHHH', buf) r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7]) = struct.unpack('<4f8H', buf)
m[0] = m0 m[0] = m0
m[1] = m1 m[1] = m1
m[2] = m2 m[2] = m2

24
libraries/Desktop/support/sitl.cpp
View File

@ -119,11 +119,8 @@ static void sitl_fgear_input(void)
static uint32_t last_report; static uint32_t last_report;
static uint32_t count; static uint32_t count;
/* sigh, its big-endian */ if (d.fg_pkt.magic != 0x4c56414e) {
swap_doubles(&d.fg_pkt.latitude, 16); printf("Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic);
d.fg_pkt.magic = ntohl(d.fg_pkt.magic);
if (d.fg_pkt.magic != 0x4c56414d) {
printf("Bad fgear packet - magic=0x%08x\n", d.fg_pkt.magic);
return; return;
} }
@ -136,20 +133,20 @@ static void sitl_fgear_input(void)
sim_state.latitude = d.fg_pkt.latitude; sim_state.latitude = d.fg_pkt.latitude;
sim_state.longitude = d.fg_pkt.longitude; sim_state.longitude = d.fg_pkt.longitude;
sim_state.altitude = ft2m(d.fg_pkt.altitude); sim_state.altitude = d.fg_pkt.altitude;
sim_state.speedN = ft2m(d.fg_pkt.speedN); sim_state.speedN = d.fg_pkt.speedN;
sim_state.speedE = ft2m(d.fg_pkt.speedE); sim_state.speedE = d.fg_pkt.speedE;
sim_state.roll = d.fg_pkt.rollDeg; sim_state.roll = d.fg_pkt.rollDeg;
sim_state.pitch = d.fg_pkt.pitchDeg; sim_state.pitch = d.fg_pkt.pitchDeg;
sim_state.yaw = d.fg_pkt.yawDeg; sim_state.yaw = d.fg_pkt.yawDeg;
sim_state.rollRate = d.fg_pkt.rollRate; sim_state.rollRate = d.fg_pkt.rollRate;
sim_state.pitchRate = d.fg_pkt.pitchRate; sim_state.pitchRate = d.fg_pkt.pitchRate;
sim_state.yawRate = d.fg_pkt.yawRate; sim_state.yawRate = d.fg_pkt.yawRate;
sim_state.xAccel = ft2m(d.fg_pkt.xAccel); sim_state.xAccel = d.fg_pkt.xAccel;
sim_state.yAccel = ft2m(d.fg_pkt.yAccel); sim_state.yAccel = d.fg_pkt.yAccel;
sim_state.zAccel = ft2m(d.fg_pkt.zAccel); sim_state.zAccel = d.fg_pkt.zAccel;
sim_state.heading = d.fg_pkt.heading; sim_state.heading = d.fg_pkt.heading;
sim_state.airspeed = kt2mps(d.fg_pkt.airspeed); sim_state.airspeed = d.fg_pkt.airspeed;
sim_state.update_count++; sim_state.update_count++;
count++; count++;
@ -189,7 +186,7 @@ static void sitl_quadcopter_output(uint16_t pwm[8])
pkt.pwm[i] = htonl(pwm[i]); pkt.pwm[i] = htonl(pwm[i]);
} }
for (uint8_t i=0; i<4; i++) { for (uint8_t i=0; i<4; i++) {
pkt.throttle[i] = swap_float((pwm[i]-1000) / 1000.0); pkt.throttle[i] = (pwm[i]-1000) / 1000.0;
} }
sendto(sitl_fd, &pkt, sizeof(pkt), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr)); sendto(sitl_fd, &pkt, sizeof(pkt), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr));
} }
@ -205,7 +202,6 @@ static void sitl_plane_output(uint16_t pwm[8])
servo[1] = (((int)pwm[1]) - 1500)/500.0; servo[1] = (((int)pwm[1]) - 1500)/500.0;
servo[2] = (((int)pwm[3]) - 1500)/500.0; servo[2] = (((int)pwm[3]) - 1500)/500.0;
servo[3] = (pwm[2] - 1000) / 1000.0; servo[3] = (pwm[2] - 1000) / 1000.0;
swap_doubles(servo, 4);
sendto(sitl_fd, &servo, sizeof(servo), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr)); sendto(sitl_fd, &servo, sizeof(servo), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr));
} }

58
libraries/Desktop/support/util.cpp
View File

@ -16,64 +16,6 @@
#include "desktop.h" #include "desktop.h"
/*
swap one double
*/
double swap_double(double d)
{
union {
double d;
uint8_t b[8];
} in, out;
int i;
in.d = d;
for (i=0; i<8; i++) {
out.b[7-i] = in.b[i];
}
return out.d;
}
/*
swap an array of doubles
*/
void swap_doubles(double *d, unsigned count)
{
while (count--) {
*d = swap_double(*d);
d++;
}
}
/*
swap one float
*/
float swap_float(float f)
{
union {
float f;
uint8_t b[4];
} in, out;
int i;
in.f = f;
for (i=0; i<4; i++) {
out.b[3-i] = in.b[i];
}
return out.f;
}
/*
swap an array of floats
*/
void swap_floats(float *f, unsigned count)
{
while (count--) {
*f = swap_float(*f);
f++;
}
}
void set_nonblocking(int fd) void set_nonblocking(int fd)
{ {
unsigned v = fcntl(fd, F_GETFL, 0); unsigned v = fcntl(fd, F_GETFL, 0);

4
libraries/Desktop/support/util.h
View File

@ -3,10 +3,6 @@
#define kt2mps(x) ((x) * 0.514444444) #define kt2mps(x) ((x) * 0.514444444)
#define sqr(x) ((x)*(x)) #define sqr(x) ((x)*(x))
double swap_double(double d);
void swap_doubles(double *d, unsigned count);
float swap_float(float f);
void swap_floats(float *f, unsigned count);
void set_nonblocking(int fd); void set_nonblocking(int fd);
double normalise(double v, double min, double max); double normalise(double v, double min, double max);
double normalise180(double v); double normalise180(double v);