zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: moved to common code for attitude/pos update

master
Andrew Tridgell 9 years ago
parent
e6555aae6d
commit
8a98ce427c
6 changed files with 72 additions and 185 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 46
      libraries/SITL/SIM_Aircraft.cpp
  2. 3
      libraries/SITL/SIM_Aircraft.h
  3. 56
      libraries/SITL/SIM_Balloon.cpp
  4. 59
      libraries/SITL/SIM_Helicopter.cpp
  5. 51
      libraries/SITL/SIM_Multicopter.cpp
  6. 42
      libraries/SITL/SIM_Plane.cpp

46
libraries/SITL/SIM_Aircraft.cpp
Unescape View File

@ -322,4 +322,50 @@ void Aircraft::set_speedup(float speedup) @@ -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
Unescape View File

@ -152,6 +152,9 @@ protected: @@ -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
Unescape View File

@ -44,18 +44,9 @@ void Balloon::update(const struct sitl_input &input) @@ -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) @@ -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
Unescape View File

@ -54,9 +54,6 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) : @@ -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) @@ -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) @@ -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
Unescape View File

@ -197,58 +197,21 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot @@ -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);
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);
// 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
Unescape View File

@ -153,50 +153,12 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel @@ -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();
}

Write Preview
Loading…
Cancel
Save