mirror of https://github.com/ArduPilot/ardupilot
SITL: added SIM_WIND_* parameters
this allows control of the simulated wind during a flight
This commit is contained in:
Andrew Tridgell
parent
dc60fe5616
commit
b8decb4fd7
|
|
@ -53,7 +53,15 @@ def setup_template(home):
|
|||
|
||||
def process_sitl_input(buf):
|
||||
'''process control changes from SITL sim'''
|
||||
pwm = list(struct.unpack('<11H', buf))
|
||||
control = list(struct.unpack('<14H', buf))
|
||||
pwm = control[:11]
|
||||
(speed, direction, turbulance) = control[11:]
|
||||
|
||||
global wind
|
||||
wind.speed = speed*0.01
|
||||
wind.direction = direction*0.01
|
||||
wind.turbulance = turbulance*0.01
|
||||
|
||||
aileron = (pwm[0]-1500)/500.0
|
||||
elevator = (pwm[1]-1500)/500.0
|
||||
throttle = (pwm[2]-1000)/1000.0
|
||||
|
|
@ -237,7 +245,7 @@ def main_loop():
|
|||
frame_count += 1
|
||||
|
||||
if sim_in.fileno() in rin:
|
||||
simbuf = sim_in.recv(22)
|
||||
simbuf = sim_in.recv(28)
|
||||
process_sitl_input(simbuf)
|
||||
last_sim_input = tnow
|
||||
|
||||
|
|
|
|||
|
|
@ -56,18 +56,29 @@ def sim_send(m, a):
|
|||
def sim_recv(m):
|
||||
'''receive control information from SITL'''
|
||||
try:
|
||||
buf = sim_in.recv(22)
|
||||
buf = sim_in.recv(28)
|
||||
except socket.error as e:
|
||||
if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
|
||||
raise
|
||||
return
|
||||
|
||||
if len(buf) != 22:
|
||||
if len(buf) != 28:
|
||||
return
|
||||
pwm = list(struct.unpack('<11H', buf))
|
||||
control = list(struct.unpack('<14H', buf))
|
||||
pwm = control[0:11]
|
||||
|
||||
# update motors
|
||||
for i in range(11):
|
||||
m[i] = (pwm[i]-1000)/1000.0
|
||||
|
||||
# update wind
|
||||
global a
|
||||
(speed, direction, turbulance) = control[11:]
|
||||
a.wind.speed = speed*0.01
|
||||
a.wind.direction = direction*0.01
|
||||
a.wind.turbulance = turbulance*0.01
|
||||
|
||||
|
||||
|
||||
def interpret_address(addrstr):
|
||||
'''interpret a IP:port string'''
|
||||
|
|
|
|||
|
|
@ -153,7 +153,10 @@ float sitl_motor_speed[4] = {0,0,0,0};
|
|||
static void sitl_simulator_output(void)
|
||||
{
|
||||
static uint32_t last_update;
|
||||
uint16_t pwm[11];
|
||||
struct {
|
||||
uint16_t pwm[11];
|
||||
uint16_t speed, direction, turbulance;
|
||||
} control;
|
||||
/* this maps the registers used for PWM outputs. The RC
|
||||
* driver updates these whenever it wants the channel output
|
||||
* to change */
|
||||
|
|
@ -180,29 +183,38 @@ static void sitl_simulator_output(void)
|
|||
|
||||
for (i=0; i<11; i++) {
|
||||
if (*reg[i] == 0xFFFF) {
|
||||
pwm[i] = 0;
|
||||
control.pwm[i] = 0;
|
||||
} else {
|
||||
pwm[i] = (*reg[i])/2;
|
||||
control.pwm[i] = (*reg[i])/2;
|
||||
}
|
||||
}
|
||||
|
||||
if (!desktop_state.quadcopter) {
|
||||
// add in engine multiplier
|
||||
if (pwm[2] > 1000) {
|
||||
pwm[2] = ((pwm[2]-1000) * sitl.engine_mul) + 1000;
|
||||
if (pwm[2] > 2000) pwm[2] = 2000;
|
||||
if (control.pwm[2] > 1000) {
|
||||
control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000;
|
||||
if (control.pwm[2] > 2000) control.pwm[2] = 2000;
|
||||
}
|
||||
|
||||
// 400kV motor, 16V
|
||||
sitl_motor_speed[0] = ((pwm[2]-1000)/1000.0) * 400 * 16 / 60.0;
|
||||
sitl_motor_speed[0] = ((control.pwm[2]-1000)/1000.0) * 400 * 16 / 60.0;
|
||||
} else {
|
||||
// 850kV motor, 16V
|
||||
for (i=0; i<4; i++) {
|
||||
sitl_motor_speed[i] = ((pwm[i]-1000)/1000.0) * 850 * 12 / 60.0;
|
||||
sitl_motor_speed[i] = ((control.pwm[i]-1000)/1000.0) * 850 * 12 / 60.0;
|
||||
}
|
||||
}
|
||||
|
||||
sendto(sitl_fd, (void*)pwm, sizeof(pwm), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr));
|
||||
// setup wind control
|
||||
control.speed = sitl.wind_speed * 100;
|
||||
float direction = sitl.wind_direction;
|
||||
if (direction < 0) {
|
||||
direction += 360;
|
||||
}
|
||||
control.direction = direction * 100;
|
||||
control.turbulance = sitl.wind_turbulance * 100;
|
||||
|
||||
sendto(sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr));
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -24,6 +24,9 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
|
|||
AP_GROUPINFO("DRIFT_TIME", 6, SITL, drift_time, 5),
|
||||
AP_GROUPINFO("GPS_DELAY", 7, SITL, gps_delay, 4),
|
||||
AP_GROUPINFO("ENGINE_MUL", 8, SITL, engine_mul, 1),
|
||||
AP_GROUPINFO("WIND_SPD", 9, SITL, wind_speed, 5),
|
||||
AP_GROUPINFO("WIND_DIR", 10, SITL, wind_direction, 180),
|
||||
AP_GROUPINFO("WIND_TURB", 11, SITL, wind_turbulance, 0.2),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -27,16 +27,6 @@ struct sitl_fdm {
|
|||
class SITL
|
||||
{
|
||||
public:
|
||||
SITL() {
|
||||
baro_noise = 3; // Pascals
|
||||
gyro_noise = 30; // degrees/s
|
||||
accel_noise = 3; // m/s/s
|
||||
mag_noise = 10; // mag units
|
||||
aspd_noise = 2; // m/s
|
||||
drift_speed = 0.2; // dps/min
|
||||
drift_time = 5; // minutes
|
||||
gps_delay = 4; // 0.8 seconds
|
||||
}
|
||||
struct sitl_fdm state;
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
|
@ -54,6 +44,11 @@ public:
|
|||
AP_Int8 gps_disable; // disable simulated GPS
|
||||
AP_Int8 gps_delay; // delay in samples
|
||||
|
||||
// wind control
|
||||
AP_Float wind_speed;
|
||||
AP_Float wind_direction;
|
||||
AP_Float wind_turbulance;
|
||||
|
||||
void simstate_send(mavlink_channel_t chan);
|
||||
|
||||
// convert a set of roll rates from earth frame to body frame
|
||||
|
|
|
|||
Loading…
Reference in New Issue