From a0baf71322589ac8dc1e2284568fa6903ed595d6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Dec 2011 22:12:58 +1100 Subject: [PATCH] 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 --- Tools/autotest/jsbsim/runsim.py | 20 +++++------ Tools/autotest/pysim/sim_quad.py | 14 ++++---- libraries/Desktop/support/sitl.cpp | 24 ++++++------- libraries/Desktop/support/util.cpp | 58 ------------------------------ libraries/Desktop/support/util.h | 4 --- 5 files changed, 27 insertions(+), 93 deletions(-) diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py index 21d25da4b8..9e18f328f2 100755 --- a/Tools/autotest/jsbsim/runsim.py +++ b/Tools/autotest/jsbsim/runsim.py @@ -50,7 +50,7 @@ def setup_home(home): def process_sitl_input(buf): '''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: jsb_set('fcs/aileron-cmd-norm', aileron) sitl_state.aileron = aileron @@ -78,24 +78,24 @@ def process_jsb_input(buf): if e.errno not in [ 111 ]: raise - simbuf = struct.pack('>ddddddddddddddddI', + simbuf = struct.pack('<16dI', fdm.get('latitude', units='degrees'), fdm.get('longitude', units='degrees'), - fdm.get('altitude', units='feet'), + fdm.get('altitude', units='meters'), fdm.get('psi', units='degrees'), - fdm.get('v_north', units='fps'), - fdm.get('v_east', units='fps'), - fdm.get('A_X_pilot', units='fpss'), - fdm.get('A_Y_pilot', units='fpss'), - fdm.get('A_Z_pilot', units='fpss'), + fdm.get('v_north', units='mps'), + fdm.get('v_east', units='mps'), + fdm.get('A_X_pilot', units='mpss'), + fdm.get('A_Y_pilot', units='mpss'), + fdm.get('A_Z_pilot', units='mpss'), fdm.get('phidot', units='dps'), fdm.get('thetadot', units='dps'), fdm.get('psidot', units='dps'), fdm.get('phi', units='degrees'), fdm.get('theta', units='degrees'), fdm.get('psi', units='degrees'), - fdm.get('vcas', units='knots'), - 0x4c56414d) + fdm.get('vcas', units='mps'), + 0x4c56414e) try: sim_out.send(simbuf) except socket.error as e: diff --git a/Tools/autotest/pysim/sim_quad.py b/Tools/autotest/pysim/sim_quad.py index aafe7103b8..d1456ebcf1 100755 --- a/Tools/autotest/pysim/sim_quad.py +++ b/Tools/autotest/pysim/sim_quad.py @@ -38,14 +38,14 @@ def sim_send(m, a, r): if not e.errno in [ errno.ECONNREFUSED ]: raise - buf = struct.pack('>ddddddddddddddddI', - a.latitude, a.longitude, util.m2ft(a.altitude), a.yaw, - util.m2ft(a.velocity.x), util.m2ft(a.velocity.y), - util.m2ft(a.accelerometer.x), util.m2ft(a.accelerometer.y), util.m2ft(a.accelerometer.z), + buf = struct.pack('<16dI', + a.latitude, a.longitude, a.altitude, a.yaw, + a.velocity.x, a.velocity.y, + a.accelerometer.x, a.accelerometer.y, a.accelerometer.z, a.roll_rate, a.pitch_rate, a.yaw_rate, a.roll, a.pitch, a.yaw, - util.m2ft(math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y)), - 0x4c56414d) + math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), + 0x4c56414e) try: sim_out.send(buf) except socket.error as e: @@ -70,7 +70,7 @@ def sim_recv(m, a, r): if len(buf) != 32: return (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[1] = m1 m[2] = m2 diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 9acc99750a..818b823fbf 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -119,11 +119,8 @@ static void sitl_fgear_input(void) static uint32_t last_report; static uint32_t count; - /* sigh, its big-endian */ - swap_doubles(&d.fg_pkt.latitude, 16); - 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); + if (d.fg_pkt.magic != 0x4c56414e) { + printf("Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic); return; } @@ -136,20 +133,20 @@ static void sitl_fgear_input(void) sim_state.latitude = d.fg_pkt.latitude; sim_state.longitude = d.fg_pkt.longitude; - sim_state.altitude = ft2m(d.fg_pkt.altitude); - sim_state.speedN = ft2m(d.fg_pkt.speedN); - sim_state.speedE = ft2m(d.fg_pkt.speedE); + sim_state.altitude = d.fg_pkt.altitude; + sim_state.speedN = d.fg_pkt.speedN; + sim_state.speedE = d.fg_pkt.speedE; sim_state.roll = d.fg_pkt.rollDeg; sim_state.pitch = d.fg_pkt.pitchDeg; sim_state.yaw = d.fg_pkt.yawDeg; sim_state.rollRate = d.fg_pkt.rollRate; sim_state.pitchRate = d.fg_pkt.pitchRate; sim_state.yawRate = d.fg_pkt.yawRate; - sim_state.xAccel = ft2m(d.fg_pkt.xAccel); - sim_state.yAccel = ft2m(d.fg_pkt.yAccel); - sim_state.zAccel = ft2m(d.fg_pkt.zAccel); + sim_state.xAccel = d.fg_pkt.xAccel; + sim_state.yAccel = d.fg_pkt.yAccel; + sim_state.zAccel = d.fg_pkt.zAccel; 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++; count++; @@ -189,7 +186,7 @@ static void sitl_quadcopter_output(uint16_t pwm[8]) pkt.pwm[i] = htonl(pwm[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)); } @@ -205,7 +202,6 @@ static void sitl_plane_output(uint16_t pwm[8]) servo[1] = (((int)pwm[1]) - 1500)/500.0; servo[2] = (((int)pwm[3]) - 1500)/500.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)); } diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp index e0f757f767..6232f00aac 100644 --- a/libraries/Desktop/support/util.cpp +++ b/libraries/Desktop/support/util.cpp @@ -16,64 +16,6 @@ #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) { unsigned v = fcntl(fd, F_GETFL, 0); diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h index 7b815c6ae6..7a028f16df 100644 --- a/libraries/Desktop/support/util.h +++ b/libraries/Desktop/support/util.h @@ -3,10 +3,6 @@ #define kt2mps(x) ((x) * 0.514444444) #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); double normalise(double v, double min, double max); double normalise180(double v);