Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Add angular acceleration to simulator states

Browse Source 
The calculation used is an approximation to work around the lack of angular acceleration in the interface with the flight dynamics model.
This commit is contained in:
priseborough 2016-10-17 08:58:59 +11:00 committed by Andrew Tridgell
parent 747f3a8cfd
commit d830f68901
3 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/SITL/SIM_Aircraft.cpp
View File

@ -42,6 +42,8 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
frame_height(0),
dcm(),
gyro(),
gyro_prev(),
ang_accel(),
velocity_ef(),
mass(0),
accel_body(0, 0, -GRAVITY_MSS),
@ -331,6 +333,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.rollRate = degrees(gyro.x);
fdm.pitchRate = degrees(gyro.y);
fdm.yawRate = degrees(gyro.z);
fdm.angAccel.x = degrees(ang_accel.x);
fdm.angAccel.y = degrees(ang_accel.y);
fdm.angAccel.z = degrees(ang_accel.z);
float r, p, y;
dcm.to_euler(&r, &p, &y);
fdm.rollDeg = degrees(r);
@ -408,6 +413,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
gyro.y = constrain_float(gyro.y, -radians(2000), radians(2000));
gyro.z = constrain_float(gyro.z, -radians(2000), radians(2000));
// estimate angular acceleration using a first order difference calculation
// TODO the simulator interface should provide the angular acceleration
ang_accel = (gyro - gyro_prev) / delta_time;
gyro_prev = gyro;
// update attitude
dcm.rotate(gyro * delta_time);
dcm.normalize();

2
libraries/SITL/SIM_Aircraft.h
View File

@ -114,6 +114,8 @@ protected:
float frame_height;
Matrix3f dcm; // rotation matrix, APM conventions, from body to earth
Vector3f gyro; // rad/s
Vector3f gyro_prev; // rad/s
Vector3f ang_accel; // rad/s/s
Vector3f velocity_ef; // m/s, earth frame
Vector3f wind_ef; // m/s, earth frame
Vector3f velocity_air_ef; // velocity relative to airmass, earth frame

1
libraries/SITL/SITL.h
View File

@ -24,6 +24,7 @@ struct sitl_fdm {
uint8_t rcin_chan_count;
float rcin[8]; // RC input 0..1
Vector3f bodyMagField; // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes
};
// number of rc output channels