diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh index 48ebf99008..2602a8c8ff 100755 --- a/Tools/autotest/sim_vehicle.sh +++ b/Tools/autotest/sim_vehicle.sh @@ -252,6 +252,16 @@ case $FRAME in BUILD_TARGET="sitl-heli" MODEL="heli" ;; + heli-dual) + BUILD_TARGET="sitl-heli-dual" + EXTRA_SIM="$EXTRA_SIM --frame=heli-dual" + MODEL="heli-dual" + ;; + heli-compound) + BUILD_TARGET="sitl-heli-compound" + EXTRA_SIM="$EXTRA_SIM --frame=heli-compound" + MODEL="heli-compound" + ;; IrisRos) BUILD_TARGET="sitl" ;; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 6361090533..9dc402c4e1 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -53,20 +53,22 @@ static const struct { const char *name; Aircraft *(*constructor)(const char *home_str, const char *frame_str); } model_constructors[] = { - { "+", MultiCopter::create }, - { "quad", MultiCopter::create }, - { "copter", MultiCopter::create }, - { "x", MultiCopter::create }, - { "hexa", MultiCopter::create }, - { "octa", MultiCopter::create }, - { "heli", Helicopter::create }, - { "rover", Rover::create }, - { "crrcsim", CRRCSim::create }, - { "jsbsim", JSBSim::create }, - { "gazebo", Gazebo::create }, - { "last_letter", last_letter::create }, - { "tracker", Tracker::create }, - { "balloon", Balloon::create } + { "+", MultiCopter::create }, + { "quad", MultiCopter::create }, + { "copter", MultiCopter::create }, + { "x", MultiCopter::create }, + { "hexa", MultiCopter::create }, + { "octa", MultiCopter::create }, + { "heli", Helicopter::create }, + { "heli-dual", Helicopter::create }, + { "heli-compound", Helicopter::create }, + { "rover", Rover::create }, + { "crrcsim", CRRCSim::create }, + { "jsbsim", JSBSim::create }, + { "gazebo", Gazebo::create }, + { "last_letter", last_letter::create }, + { "tracker", Tracker::create }, + { "balloon", Balloon::create } }; void SITL_State::_parse_command_line(int argc, char * const argv[]) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index f5290dfaa6..c63143b5a2 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -50,6 +50,7 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) : accel_noise(0.3), rate_hz(400), autotest_dir(NULL), + frame(frame_str), #ifdef __CYGWIN__ min_sleep_time(20000) #else diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 32b680115e..f029b973f7 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -98,6 +98,7 @@ protected: uint64_t last_wall_time_us; uint8_t instance; const char *autotest_dir; + const char *frame; bool on_ground(const Vector3f &pos) const; diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index d693d7237f..48b1aa34bc 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -47,24 +47,66 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) : */ void Helicopter::update(const struct sitl_input &input) { - float swash1 = (input.servos[0]-1000) / 1000.0f; - float swash2 = (input.servos[1]-1000) / 1000.0f; - float swash3 = (input.servos[2]-1000) / 1000.0f; - float tail_rotor = (input.servos[3]-1000) / 1000.0f; - float rsc = (input.servos[7]-1000) / 1000.0f; - // how much time has passed? float delta_time = frame_time_us * 1.0e-6f; - float thrust = (rsc/rsc_setpoint)*(swash1+swash2+swash3)/3.0f; - - // very simplistic mapping to body euler rates - float roll_rate = swash1 - swash2; - float pitch_rate = (swash1 + swash2)/2.0f - swash3; - float yaw_rate = tail_rotor - 0.5f; - + float rsc = (input.servos[7]-1000) / 1000.0f; float rsc_scale = rsc/rsc_setpoint; + float thrust = 0; + float roll_rate = 0; + float pitch_rate = 0; + float yaw_rate = 0; + float torque_effect_accel = 0; + float lateral_x_thrust = 0; + float lateral_y_thrust = 0; + + float swash1 = (input.servos[0]-1000) / 1000.0f; + float swash2 = (input.servos[1]-1000) / 1000.0f; + float swash3 = (input.servos[2]-1000) / 1000.0f; + + if (strcmp(frame, "heli") == 0) { + // simulate a traditional helicopter + + float tail_rotor = (input.servos[3]-1000) / 1000.0f; + + thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; + torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; + + roll_rate = swash1 - swash2; + pitch_rate = (swash1+swash2) / 2.0f - swash3; + yaw_rate = tail_rotor - 0.5f; + + lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale; + } else if (strcmp(frame, "heli-dual") == 0) { + // simulate a tandem helicopter + + float swash4 = (input.servos[3]-1000) / 1000.0f; + float swash5 = (input.servos[4]-1000) / 1000.0f; + float swash6 = (input.servos[5]-1000) / 1000.0f; + + thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f; + torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6)); + + roll_rate = (swash1-swash2) + (swash4-swash5); + pitch_rate = (swash1+swash2+swash3) - (swash4+swash5+swash6); + yaw_rate = (swash1-swash2) + (swash5-swash4); + } else if (strcmp(frame, "heli-compound") == 0) { + // simulate a compound helicopter + + float right_rotor = (input.servos[3]-1000) / 1000.0f; + float left_rotor = (input.servos[4]-1000) / 1000.0f; + + thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; + torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; + + roll_rate = swash1 - swash2; + pitch_rate = (swash1+swash2) / 2.0f - swash3; + yaw_rate = right_rotor - left_rotor; + + lateral_x_thrust = (left_rotor+right_rotor-1) * rsc_scale * tail_thrust_scale; + } + roll_rate *= rsc_scale; pitch_rate *= rsc_scale; yaw_rate *= rsc_scale; @@ -81,7 +123,7 @@ void Helicopter::update(const struct sitl_input &input) rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; // torque effect on tail - rot_accel.z += (rsc_scale+thrust) * rotor_rot_accel; + rot_accel.z += torque_effect_accel; // update rotational rates in body frame gyro += rot_accel * delta_time; @@ -96,7 +138,7 @@ void Helicopter::update(const struct sitl_input &input) // scale thrust to newtons thrust *= thrust_scale; - accel_body = Vector3f(0, yaw_rate * rsc_scale * tail_thrust_scale, -thrust / mass); + accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass); Vector3f accel_earth = dcm * accel_body; accel_earth += Vector3f(0, 0, GRAVITY_MSS); accel_earth += air_resistance;