Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: moved to common code for attitude/pos update

This commit is contained in:
Andrew Tridgell 2016-01-01 15:11:55 +11:00
parent e6555aae6d
commit 8a98ce427c
6 changed files with 72 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/SITL/SIM_Aircraft.cpp
View File

@ -322,4 +322,50 @@ void Aircraft::set_speedup(float speedup)
setup_frame_time(rate_hz, speedup);
}
/*
update the simulation attitude and relative position
*/
void Aircraft::update_dynamics(const Vector3f &rot_accel)
{
float delta_time = frame_time_us * 1.0e-6f;
// update rotational rates in body frame
gyro += rot_accel * delta_time;
// update attitude
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);
}
}
}
} // namespace SITL

3
libraries/SITL/SIM_Aircraft.h
View File

@ -152,6 +152,9 @@ protected:
/* return wall clock time in microseconds since 1970 */
uint64_t get_wall_time_us(void) const;
// update attitude and relative position
void update_dynamics(const Vector3f &rot_accel);
private:
uint64_t last_time_us = 0;
uint32_t frame_counter = 0;

56
libraries/SITL/SIM_Balloon.cpp
View File

@ -44,18 +44,9 @@ void Balloon::update(const struct sitl_input &input)
burst = true;
}
float delta_time = frame_time_us * 1.0e-6f;
// rotational air resistance
Vector3f rot_accel = -gyro * radians(400) / terminal_rotation_rate;
// update rotational rates in body frame
gyro += rot_accel * delta_time;
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// air resistance
Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
@ -66,54 +57,15 @@ void Balloon::update(const struct sitl_input &input)
}
accel_body = Vector3f(0, 0, -lift_accel);
Vector3f accel_earth = dcm * accel_body;
accel_earth += Vector3f(0, 0, GRAVITY_MSS);
accel_earth += air_resistance;
// 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));
// add some noise
add_noise(velocity_ef.length() / terminal_velocity);
// new velocity vector
velocity_ef += accel_earth * delta_time;
// new position vector
Vector3f old_position = position;
position += velocity_ef * delta_time;
accel_body += dcm * air_resistance;
update_dynamics(rot_accel);
if (position.z < -burst_altitude) {
::printf("Balloon burst at %.1f\n", -position.z);
burst = true;
}
// 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);
velocity_ef.zero();
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
dcm.from_euler(0, 0, y);
position.z = -(ground_level + frame_height - home.alt*0.01f);
}
}
// update lat/lon/altitude
update_position();
}

59
libraries/SITL/SIM_Helicopter.cpp
View File

@ -54,9 +54,6 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) :
*/
void Helicopter::update(const struct sitl_input &input)
{
// how much time has passed?
float delta_time = frame_time_us * 1.0e-6f;
float rsc = (input.servos[7]-1000) / 1000.0f;
// ignition only for gas helis
bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true;
@ -147,13 +144,6 @@ void Helicopter::update(const struct sitl_input &input)
// torque effect on tail
rot_accel.z += torque_effect_accel;
// update rotational rates in body frame
gyro += rot_accel * delta_time;
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();
// air resistance
Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
@ -161,47 +151,18 @@ void Helicopter::update(const struct sitl_input &input)
thrust *= thrust_scale;
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;
// 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));
// add some noise
add_noise(thrust / thrust_scale);
// 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();
accel_body += dcm * air_resistance;
update_dynamics(rot_accel);
// 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);
velocity_ef.zero();
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
dcm.from_euler(0, 0, y);
position.z = -(ground_level + frame_height - home.alt*0.01f);
}
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
dcm.from_euler(0, 0, y);
position.z = -(ground_level + frame_height - home.alt*0.01f);
}
// update lat/lon/altitude

51
libraries/SITL/SIM_Multicopter.cpp
View File

@ -197,58 +197,21 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot
void MultiCopter::update(const struct sitl_input &input)
{
// how much time has passed?
float delta_time = frame_time_us * 1.0e-6f;
Vector3f rot_accel;
calculate_forces(input, rot_accel, accel_body);
// update rotational rates in body frame
gyro += rot_accel * delta_time;
update_dynamics(rot_accel);
// update attitude
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);
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
dcm.from_euler(0, 0, y);
velocity_ef.zero();
// zero roll/pitch, but keep yaw
float r, p, y;
dcm.to_euler(&r, &p, &y);
dcm.from_euler(0, 0, y);
position.z = -(ground_level + frame_height - home.alt*0.01f);
}
position.z = -(ground_level + frame_height - home.alt*0.01f);
}
// update lat/lon/altitude
update_position();
}

42
libraries/SITL/SIM_Plane.cpp
View File

@ -153,50 +153,12 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
*/
void Plane::update(const struct sitl_input &input)
{
float delta_time = frame_time_us * 1.0e-6f;
Vector3f rot_accel;
calculate_forces(input, rot_accel, accel_body);
// update rotational rates in body frame
gyro += rot_accel * delta_time;
// update attitude
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_dynamics(rot_accel);
// update lat/lon/altitude
update_position();
}