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

SITL: added very simple tiltrotor simulation

Browse Source 
uses channel 9 to control tilt of rotors
This commit is contained in:
Andrew Tridgell 2016-01-30 09:43:10 +11:00
parent c85f9e8b44
commit e62d6711c3
3 changed files with 35 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/SITL/SIM_Multicopter.cpp
View File

@ -161,17 +161,21 @@ void Frame::calculate_forces(const Aircraft &aircraft,
thrust += motor_speed[i] * thrust_scale; // newtons
}
// rotational air resistance
const Vector3f &gyro = aircraft.get_gyro();
rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
// air resistance
Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
body_accel = Vector3f(0, 0, -thrust / mass);
body_accel += aircraft.get_dcm().transposed() * air_resistance;
if (terminal_rotation_rate > 0) {
// rotational air resistance
const Vector3f &gyro = aircraft.get_gyro();
rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
}
if (terminal_velocity > 0) {
// air resistance
Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
body_accel += aircraft.get_dcm().transposed() * air_resistance;
}
// add some noise
const float gyro_noise = radians(0.1);

22
libraries/SITL/SIM_QuadPlane.cpp
View File

@ -37,8 +37,13 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
Plane(home_str, frame_str)
{
frame = &quad_frame;
// we use a very high terminal velocity to let the plane model handle the drag
frame->init(mass, 0.51, 100, 20*radians(360));
// we use zero terminal velocity to let the plane model handle the drag
frame->init(mass, 0.51, 0, 0);
// see if we want a tiltrotor
if (strstr(frame_str, "-tilt")) {
tiltrotors = true;
}
}
/*
@ -56,6 +61,19 @@ void QuadPlane::update(const struct sitl_input &input)
frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body);
if (tiltrotors) {
// get rotor tilt from channel 9
float tilt = constrain_float(-90 * (input.servos[8]-1000)*0.001, -90, 0);
// rotate the rotational accel and body accel from multicopter
// model to take account of rotor tilt
Matrix3f rotation;
rotation.identity();
rotation.rotate(Vector3f(0, radians(tilt), 0));
quad_rot_accel = rotation * quad_rot_accel;
quad_accel_body = rotation * quad_accel_body;
}
rot_accel += quad_rot_accel;
accel_body += quad_accel_body;

2
libraries/SITL/SIM_QuadPlane.h
View File

@ -41,7 +41,7 @@ public:
}
private:
Frame *frame;
bool tiltrotors;
};
} // namespace SITL