aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: Add HELI_DUAL_FRAME and HELI_COMPOUND_FRAME.

Browse Source
This commit is contained in:
Fredrik Hedberg 2015-06-02 01:08:55 +02:00 committed by Randy Mackay
parent f10f4a82ba
commit 3ea31099d7
5 changed files with 85 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Tools/autotest/sim_vehicle.sh
View File

@ -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"
;;

30
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -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[])

1
libraries/SITL/SIM_Aircraft.cpp
View File

@ -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

1
libraries/SITL/SIM_Aircraft.h
View File

@ -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;

72
libraries/SITL/SIM_Helicopter.cpp
View File

@ -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;