diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py index 9e18f328f2..e4e2129887 100755 --- a/Tools/autotest/jsbsim/runsim.py +++ b/Tools/autotest/jsbsim/runsim.py @@ -50,7 +50,12 @@ def setup_home(home): def process_sitl_input(buf): '''process control changes from SITL sim''' - (aileron, elevator, rudder, throttle) = struct.unpack('<4d', buf) + pwm = list(struct.unpack('<11H', buf)) + aileron = (pwm[0]-1500)/500.0 + elevator = (pwm[1]-1500)/500.0 + throttle = (pwm[2]-1000)/1000.0 + rudder = (pwm[3]-1500)/500.0 + if aileron != sitl_state.aileron: jsb_set('fcs/aileron-cmd-norm', aileron) sitl_state.aileron = aileron @@ -212,7 +217,7 @@ def main_loop(): frame_count += 1 if sim_in.fileno() in rin: - simbuf = sim_in.recv(32) + simbuf = sim_in.recv(22) process_sitl_input(simbuf) # show any jsbsim console output diff --git a/Tools/autotest/pysim/sim_quad.py b/Tools/autotest/pysim/sim_quad.py index d1456ebcf1..641f1fd559 100755 --- a/Tools/autotest/pysim/sim_quad.py +++ b/Tools/autotest/pysim/sim_quad.py @@ -13,7 +13,7 @@ for d in [ 'pymavlink', import mavlink -def sim_send(m, a, r): +def sim_send(m, a): '''send flight information to mavproxy and flightgear''' global fdm @@ -53,7 +53,7 @@ def sim_send(m, a, r): raise -def sim_recv(m, a, r): +def sim_recv(m, a): '''receive control information from SITL''' while True: fd = sim_in.fileno() @@ -66,15 +66,12 @@ def sim_recv(m, a, r): if fd in rin: break util.check_parent() - buf = sim_in.recv(32) - if len(buf) != 32: + buf = sim_in.recv(22) + if len(buf) != 22: return - (m0, m1, m2, m3, - 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 - m[3] = m3 + pwm = list(struct.unpack('<11H', buf)) + for i in range(4): + m[i] = (pwm[i]-1000)/1000.0 def interpret_address(addrstr): @@ -133,9 +130,6 @@ a.update_frequency = opts.rate # motors initially off m = [0, 0, 0, 0] -# raw PWM -r = [0, 0, 0, 0, 0, 0, 0, 0] - lastt = time.time() frame_count = 0 @@ -162,13 +156,12 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % ( a.yaw)) while True: - sim_recv(m, a, r) + sim_recv(m, a) - # allow for adding inbalance in flight m2 = m[:] a.update(m2) - sim_send(m, a, r) + sim_send(m, a) frame_count += 1 t = time.time() if t - lastt > 1.0: diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 818b823fbf..f0c6232b22 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -31,34 +31,43 @@ #include "desktop.h" #include "util.h" -#define FGIN_PORT 5501 -#define FGOUT_PORT 5502 +/* + the sitl_fdm packet is received by the SITL build from the flight + simulator. This is used to feed the internal sensor emulation + */ +struct sitl_fdm { + // little-endian packet format + double latitude, longitude; // degrees + double altitude; // MSL + double heading; // degrees + double speedN, speedE; // m/s + double xAccel, yAccel, zAccel; // m/s/s in body frame + double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame + double rollDeg, pitchDeg, yawDeg; // euler angles, degrees + double airspeed; // m/s + uint32_t magic; // 0x4c56414e +}; + + +#define SIMIN_PORT 5501 +#define RCOUT_PORT 5502 static int sitl_fd; -struct sockaddr_in fgout_addr; +struct sockaddr_in rcout_addr; static pid_t parent_pid; struct ADC_UDR2 UDR2; struct RC_ICR4 ICR4; extern AP_TimerProcess timer_scheduler; extern Arduino_Mega_ISR_Registry isr_registry; -static volatile struct { - double latitude, longitude; // degrees - double altitude; // MSL - double heading; // degrees - double speedN, speedE; // m/s - double roll, pitch, yaw; // degrees - double rollRate, pitchRate, yawRate; // degrees per second - double xAccel, yAccel, zAccel; // meters/s/s - double airspeed; // m/s - uint32_t update_count; -} sim_state; +static struct sitl_fdm sim_state; +static uint32_t update_count; /* - setup a FGear listening UDP port, using protocol from MAVLink.xml + setup a SITL FDM listening UDP port */ -static void setup_fgear(void) +static void setup_fdm(void) { int one=1, ret; struct sockaddr_in sockaddr; @@ -68,7 +77,7 @@ static void setup_fgear(void) #ifdef HAVE_SOCK_SIN_LEN sockaddr.sin_len = sizeof(sockaddr); #endif - sockaddr.sin_port = htons(FGIN_PORT); + sockaddr.sin_port = htons(SIMIN_PORT); sockaddr.sin_family = AF_INET; sitl_fd = socket(AF_INET, SOCK_DGRAM, 0); @@ -91,25 +100,16 @@ static void setup_fgear(void) } /* - check for a fgear packet + check for a SITL FDM packet */ -static void sitl_fgear_input(void) +static void sitl_fdm_input(void) { ssize_t size; - struct fg_mavlink { - double latitude, longitude, altitude, heading, - speedN, speedE, - xAccel, yAccel, zAccel, - rollRate, pitchRate, yawRate, - rollDeg, pitchDeg, yawDeg, - airspeed; - uint32_t magic; - }; struct pwm_packet { uint16_t pwm[8]; }; union { - struct fg_mavlink fg_pkt; + struct sitl_fdm fg_pkt; struct pwm_packet pwm_pkt; } d; @@ -131,23 +131,8 @@ static void sitl_fgear_input(void) return; } - sim_state.latitude = d.fg_pkt.latitude; - sim_state.longitude = d.fg_pkt.longitude; - 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 = 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 = d.fg_pkt.airspeed; - sim_state.update_count++; + sim_state = d.fg_pkt; + update_count++; count++; if (millis() - last_report > 1000) { @@ -173,46 +158,13 @@ static void sitl_fgear_input(void) } -/* - send RC outputs to simulator for a quadcopter - */ -static void sitl_quadcopter_output(uint16_t pwm[8]) -{ - struct fg_output { - float throttle[4]; - uint16_t pwm[8]; - } pkt; - for (uint8_t i=0; i<8; i++) { - pkt.pwm[i] = htonl(pwm[i]); - } - for (uint8_t i=0; i<4; i++) { - pkt.throttle[i] = (pwm[i]-1000) / 1000.0; - } - sendto(sitl_fd, &pkt, sizeof(pkt), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr)); -} - -/* - send RC outputs to simulator for a plane - */ -static void sitl_plane_output(uint16_t pwm[8]) -{ - double servo[4]; - - servo[0] = (((int)pwm[0]) - 1500)/500.0; - servo[1] = (((int)pwm[1]) - 1500)/500.0; - servo[2] = (((int)pwm[3]) - 1500)/500.0; - servo[3] = (pwm[2] - 1000) / 1000.0; - sendto(sitl_fd, &servo, sizeof(servo), MSG_DONTWAIT, (const sockaddr *)&fgout_addr, sizeof(fgout_addr)); -} - - /* send RC outputs to simulator for a quadcopter */ static void sitl_simulator_output(void) { static uint32_t last_update; - uint16_t pwm[8]; + uint16_t pwm[11]; /* this maps the registers used for PWM outputs. The RC * driver updates these whenever it wants the channel output * to change */ @@ -222,14 +174,12 @@ static void sitl_simulator_output(void) uint8_t i; if (last_update == 0) { - if (desktop_state.quadcopter) { - for (i=0; i<8; i++) { - (*reg[i]) = 1000*2; - } - } else { + for (i=0; i<11; i++) { + (*reg[i]) = 1000*2; + } + if (!desktop_state.quadcopter) { (*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2; - (*reg[2]) = (*reg[4]) = (*reg[6]) = 1000*2; - (*reg[5]) = (*reg[7]) = 1800*2; + (*reg[7]) = 1800*2; } } @@ -239,17 +189,11 @@ static void sitl_simulator_output(void) } last_update = millis(); - for (i=0; i<8; i++) { + for (i=0; i<11; i++) { // the registers are 2x the PWM value pwm[i] = (*reg[i])/2; } - - if (desktop_state.quadcopter) { - sitl_quadcopter_output(pwm); - } else { - sitl_plane_output(pwm); - } - + sendto(sitl_fd, (void*)pwm, sizeof(pwm), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr)); } /* @@ -265,7 +209,7 @@ static void timer_handler(int signum) } /* check for packet from flight sim */ - sitl_fgear_input(); + sitl_fdm_input(); // trigger all timers timer_scheduler.run(); @@ -278,25 +222,25 @@ static void timer_handler(int signum) // send RC output to flight sim sitl_simulator_output(); - if (sim_state.update_count == 0) { + if (update_count == 0) { sitl_update_gps(0, 0, 0, 0, 0, false); return; } - if (sim_state.update_count == last_update_count) { + if (update_count == last_update_count) { return; } - last_update_count = sim_state.update_count; + last_update_count = update_count; sitl_update_gps(sim_state.latitude, sim_state.longitude, sim_state.altitude, sim_state.speedN, sim_state.speedE, true); - sitl_update_adc(sim_state.roll, sim_state.pitch, sim_state.yaw, + sitl_update_adc(sim_state.rollDeg, sim_state.pitchDeg, sim_state.yawDeg, sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate, sim_state.xAccel, sim_state.yAccel, sim_state.zAccel, sim_state.airspeed); sitl_update_barometer(sim_state.altitude); - sitl_update_compass(sim_state.heading, sim_state.roll, sim_state.pitch, sim_state.heading); + sitl_update_compass(sim_state.heading, sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading); } @@ -329,12 +273,12 @@ void sitl_setup(void) { parent_pid = getppid(); - fgout_addr.sin_family = AF_INET; - fgout_addr.sin_port = htons(FGOUT_PORT); - inet_pton(AF_INET, "127.0.0.1", &fgout_addr.sin_addr); + rcout_addr.sin_family = AF_INET; + rcout_addr.sin_port = htons(RCOUT_PORT); + inet_pton(AF_INET, "127.0.0.1", &rcout_addr.sin_addr); setup_timer(); - setup_fgear(); + setup_fdm(); sitl_setup_adc(); printf("Starting SITL input\n");