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

autotest: support all 11 PWM output channels 

this will allow for more complex aircraft
This commit is contained in:
Andrew Tridgell 2011-12-02 22:45:48 +11:00
parent a0baf71322
commit 9dd2021ec8
3 changed files with 65 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -50,7 +50,12 @@ 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('<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: 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
@ -212,7 +217,7 @@ def main_loop():
frame_count += 1 frame_count += 1
if sim_in.fileno() in rin: if sim_in.fileno() in rin:
simbuf = sim_in.recv(32) simbuf = sim_in.recv(22)
process_sitl_input(simbuf) process_sitl_input(simbuf)
# show any jsbsim console output # show any jsbsim console output

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

@ -13,7 +13,7 @@ for d in [ 'pymavlink',
import mavlink import mavlink
def sim_send(m, a, r): def sim_send(m, a):
'''send flight information to mavproxy and flightgear''' '''send flight information to mavproxy and flightgear'''
global fdm global fdm
@ -53,7 +53,7 @@ def sim_send(m, a, r):
raise raise
def sim_recv(m, a, r): def sim_recv(m, a):
'''receive control information from SITL''' '''receive control information from SITL'''
while True: while True:
fd = sim_in.fileno() fd = sim_in.fileno()
@ -66,15 +66,12 @@ def sim_recv(m, a, r):
if fd in rin: if fd in rin:
break break
util.check_parent() util.check_parent()
buf = sim_in.recv(32) buf = sim_in.recv(22)
if len(buf) != 32: if len(buf) != 22:
return return
(m0, m1, m2, m3, pwm = list(struct.unpack('<11H', buf))
r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7]) = struct.unpack('<4f8H', buf) for i in range(4):
m[0] = m0 m[i] = (pwm[i]-1000)/1000.0
m[1] = m1
m[2] = m2
m[3] = m3
def interpret_address(addrstr): def interpret_address(addrstr):
@ -133,9 +130,6 @@ a.update_frequency = opts.rate
# motors initially off # motors initially off
m = [0, 0, 0, 0] m = [0, 0, 0, 0]
# raw PWM
r = [0, 0, 0, 0, 0, 0, 0, 0]
lastt = time.time() lastt = time.time()
frame_count = 0 frame_count = 0
@ -162,13 +156,12 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % (
a.yaw)) a.yaw))
while True: while True:
sim_recv(m, a, r) sim_recv(m, a)
# allow for adding inbalance in flight
m2 = m[:] m2 = m[:]
a.update(m2) a.update(m2)
sim_send(m, a, r) sim_send(m, a)
frame_count += 1 frame_count += 1
t = time.time() t = time.time()
if t - lastt > 1.0: if t - lastt > 1.0:

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

@ -31,34 +31,43 @@
#include "desktop.h" #include "desktop.h"
#include "util.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; static int sitl_fd;
struct sockaddr_in fgout_addr; struct sockaddr_in rcout_addr;
static pid_t parent_pid; static pid_t parent_pid;
struct ADC_UDR2 UDR2; struct ADC_UDR2 UDR2;
struct RC_ICR4 ICR4; struct RC_ICR4 ICR4;
extern AP_TimerProcess timer_scheduler; extern AP_TimerProcess timer_scheduler;
extern Arduino_Mega_ISR_Registry isr_registry; extern Arduino_Mega_ISR_Registry isr_registry;
static volatile struct { static struct sitl_fdm sim_state;
double latitude, longitude; // degrees static uint32_t update_count;
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;
/* /*
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; int one=1, ret;
struct sockaddr_in sockaddr; struct sockaddr_in sockaddr;
@ -68,7 +77,7 @@ static void setup_fgear(void)
#ifdef HAVE_SOCK_SIN_LEN #ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr); sockaddr.sin_len = sizeof(sockaddr);
#endif #endif
sockaddr.sin_port = htons(FGIN_PORT); sockaddr.sin_port = htons(SIMIN_PORT);
sockaddr.sin_family = AF_INET; sockaddr.sin_family = AF_INET;
sitl_fd = socket(AF_INET, SOCK_DGRAM, 0); 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; 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 { struct pwm_packet {
uint16_t pwm[8]; uint16_t pwm[8];
}; };
union { union {
struct fg_mavlink fg_pkt; struct sitl_fdm fg_pkt;
struct pwm_packet pwm_pkt; struct pwm_packet pwm_pkt;
} d; } d;
@ -131,23 +131,8 @@ static void sitl_fgear_input(void)
return; return;
} }
sim_state.latitude = d.fg_pkt.latitude; sim_state = d.fg_pkt;
sim_state.longitude = d.fg_pkt.longitude; update_count++;
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++;
count++; count++;
if (millis() - last_report > 1000) { 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 send RC outputs to simulator for a quadcopter
*/ */
static void sitl_simulator_output(void) static void sitl_simulator_output(void)
{ {
static uint32_t last_update; static uint32_t last_update;
uint16_t pwm[8]; uint16_t pwm[11];
/* this maps the registers used for PWM outputs. The RC /* this maps the registers used for PWM outputs. The RC
* driver updates these whenever it wants the channel output * driver updates these whenever it wants the channel output
* to change */ * to change */
@ -222,14 +174,12 @@ static void sitl_simulator_output(void)
uint8_t i; uint8_t i;
if (last_update == 0) { if (last_update == 0) {
if (desktop_state.quadcopter) { for (i=0; i<11; i++) {
for (i=0; i<8; i++) {
(*reg[i]) = 1000*2; (*reg[i]) = 1000*2;
} }
} else { if (!desktop_state.quadcopter) {
(*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2; (*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2;
(*reg[2]) = (*reg[4]) = (*reg[6]) = 1000*2; (*reg[7]) = 1800*2;
(*reg[5]) = (*reg[7]) = 1800*2;
} }
} }
@ -239,17 +189,11 @@ static void sitl_simulator_output(void)
} }
last_update = millis(); last_update = millis();
for (i=0; i<8; i++) { for (i=0; i<11; i++) {
// the registers are 2x the PWM value // the registers are 2x the PWM value
pwm[i] = (*reg[i])/2; pwm[i] = (*reg[i])/2;
} }
sendto(sitl_fd, (void*)pwm, sizeof(pwm), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr));
if (desktop_state.quadcopter) {
sitl_quadcopter_output(pwm);
} else {
sitl_plane_output(pwm);
}
} }
/* /*
@ -265,7 +209,7 @@ static void timer_handler(int signum)
} }
/* check for packet from flight sim */ /* check for packet from flight sim */
sitl_fgear_input(); sitl_fdm_input();
// trigger all timers // trigger all timers
timer_scheduler.run(); timer_scheduler.run();
@ -278,25 +222,25 @@ static void timer_handler(int signum)
// send RC output to flight sim // send RC output to flight sim
sitl_simulator_output(); sitl_simulator_output();
if (sim_state.update_count == 0) { if (update_count == 0) {
sitl_update_gps(0, 0, 0, 0, 0, false); sitl_update_gps(0, 0, 0, 0, 0, false);
return; return;
} }
if (sim_state.update_count == last_update_count) { if (update_count == last_update_count) {
return; return;
} }
last_update_count = sim_state.update_count; last_update_count = update_count;
sitl_update_gps(sim_state.latitude, sim_state.longitude, sitl_update_gps(sim_state.latitude, sim_state.longitude,
sim_state.altitude, sim_state.altitude,
sim_state.speedN, sim_state.speedE, true); 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.rollRate, sim_state.pitchRate, sim_state.yawRate,
sim_state.xAccel, sim_state.yAccel, sim_state.zAccel, sim_state.xAccel, sim_state.yAccel, sim_state.zAccel,
sim_state.airspeed); sim_state.airspeed);
sitl_update_barometer(sim_state.altitude); 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(); parent_pid = getppid();
fgout_addr.sin_family = AF_INET; rcout_addr.sin_family = AF_INET;
fgout_addr.sin_port = htons(FGOUT_PORT); rcout_addr.sin_port = htons(RCOUT_PORT);
inet_pton(AF_INET, "127.0.0.1", &fgout_addr.sin_addr); inet_pton(AF_INET, "127.0.0.1", &rcout_addr.sin_addr);
setup_timer(); setup_timer();
setup_fgear(); setup_fdm();
sitl_setup_adc(); sitl_setup_adc();
printf("Starting SITL input\n"); printf("Starting SITL input\n");