From 697b2074a5160f9d06219518f6682c3ab7e06297 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Aug 2012 21:22:20 +1000 Subject: [PATCH] SITL: added SIM_WIND_* parameters this allows control of the simulated wind during a flight --- Tools/autotest/jsbsim/runsim.py | 12 ++++++++-- Tools/autotest/pysim/sim_multicopter.py | 17 +++++++++++--- libraries/Desktop/support/sitl.cpp | 30 +++++++++++++++++-------- libraries/SITL/SITL.cpp | 3 +++ libraries/SITL/SITL.h | 15 +++++-------- 5 files changed, 53 insertions(+), 24 deletions(-) diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py index 23abcc7ffe..07fcfe388e 100755 --- a/Tools/autotest/jsbsim/runsim.py +++ b/Tools/autotest/jsbsim/runsim.py @@ -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 diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py index 72ca51e665..3d408c3ae3 100755 --- a/Tools/autotest/pysim/sim_multicopter.py +++ b/Tools/autotest/pysim/sim_multicopter.py @@ -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''' diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index efc39c930a..b28114139c 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -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)); } /* diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 491f7253c5..6c22d5d59d 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -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 }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 9f7e65488c..81824d3bb1 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -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