mirror of https://github.com/ArduPilot/ardupilot
SITL: use common dynamics code for QuadPlane
This commit is contained in:
parent
2f4933ec41
commit
619a4c0925
|
@ -45,8 +45,6 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
|
||||||
*/
|
*/
|
||||||
void QuadPlane::update(const struct sitl_input &input)
|
void QuadPlane::update(const struct sitl_input &input)
|
||||||
{
|
{
|
||||||
float delta_time = frame_time_us * 1.0e-6f;
|
|
||||||
|
|
||||||
// first plane forces
|
// first plane forces
|
||||||
Vector3f rot_accel;
|
Vector3f rot_accel;
|
||||||
calculate_forces(input, rot_accel, accel_body);
|
calculate_forces(input, rot_accel, accel_body);
|
||||||
|
@ -59,44 +57,8 @@ void QuadPlane::update(const struct sitl_input &input)
|
||||||
|
|
||||||
rot_accel += quad_rot_accel;
|
rot_accel += quad_rot_accel;
|
||||||
accel_body += quad_accel_body;
|
accel_body += quad_accel_body;
|
||||||
|
|
||||||
// update rotational rates in body frame
|
|
||||||
gyro += rot_accel * delta_time;
|
|
||||||
|
|
||||||
// update attitude
|
update_dynamics(rot_accel);
|
||||||
dcm.rotate(gyro * delta_time);
|
|
||||||
dcm.normalize();
|
|
||||||
|
|
||||||
Vector3f accel_earth = dcm * accel_body;
|
|
||||||
accel_earth += Vector3f(0, 0, GRAVITY_MSS);
|
|
||||||
|
|
||||||
// if we're on the ground, then our vertical acceleration is limited
|
|
||||||
// to zero. This effectively adds the force of the ground on the aircraft
|
|
||||||
if (on_ground(position) && accel_earth.z > 0) {
|
|
||||||
accel_earth.z = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// work out acceleration as seen by the accelerometers. It sees the kinematic
|
|
||||||
// acceleration (ie. real movement), plus gravity
|
|
||||||
accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
|
|
||||||
|
|
||||||
// new velocity vector
|
|
||||||
velocity_ef += accel_earth * delta_time;
|
|
||||||
|
|
||||||
// new position vector
|
|
||||||
Vector3f old_position = position;
|
|
||||||
position += velocity_ef * delta_time;
|
|
||||||
|
|
||||||
// assume zero wind for now
|
|
||||||
airspeed = velocity_ef.length();
|
|
||||||
|
|
||||||
// constrain height to the ground
|
|
||||||
if (on_ground(position)) {
|
|
||||||
if (!on_ground(old_position)) {
|
|
||||||
printf("Hit ground at %f m/s\n", velocity_ef.z);
|
|
||||||
position.z = -(ground_level + frame_height - home.alt*0.01f);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// update lat/lon/altitude
|
// update lat/lon/altitude
|
||||||
update_position();
|
update_position();
|
||||||
|
|
Loading…
Reference in New Issue