1
0
Fork 0

Fixes a bugs in the handling of the holdback mount point (by Mathias),

and in the calculation of the launchbar angle (by Vivian).

It also calculates the holdback angle, and sets a Boolean value which
can be used to initiate the release of the catapult strop submodel at
the appropriate moment (new code by Vivian).
This commit is contained in:
andy 2006-03-01 22:59:58 +00:00
parent 47f6b67365
commit e4fe99ce12
6 changed files with 258 additions and 57 deletions

View file

@ -63,6 +63,11 @@ void Hook::getPosition(float* out)
for(i=0; i<3; i++) out[i] = _pos[i];
}
float Hook::getHookPos(int i)
{
return _pos[i];
}
float Hook::getLength(void)
{
return _length;
@ -78,6 +83,11 @@ float Hook::getUpAngle(void)
return _up_ang;
}
float Hook::getAngle(void)
{
return _ang;
}
float Hook::getExtension(void)
{
return _extension;
@ -97,8 +107,8 @@ float Hook::getCompressFraction()
void Hook::getTipPosition(float* out)
{
// The hook tip in local coordinates.
float ang = _frac*(_down_ang - _up_ang) + _up_ang;
float pos_tip[3] = { _length*Math::cos(ang), 0.0, _length*Math::sin(ang) };
_ang = _frac*(_down_ang - _up_ang) + _up_ang;
float pos_tip[3] = { _length*Math::cos(_ang), 0, _length*Math::sin(_ang) };
Math::sub3(_pos, pos_tip, out);
}

View file

@ -44,11 +44,15 @@ public:
void getForce(float* force, float* off);
float getCompressFraction(void);
float Hook::getAngle(void);
float Hook::getHookPos(int i);
private:
float _pos[3];
float _length;
float _down_ang;
float _up_ang;
float _ang;
float _extension;
float _force[3];
float _frac;

View file

@ -5,15 +5,19 @@
#include "Launchbar.hpp"
#include <iostream>
using namespace std;
namespace yasim {
static const float YASIM_PI2 = 3.14159265358979323846/2;
static const float YASIM_PI = 3.14159265358979323846;
static const float RAD2DEG = 180/YASIM_PI;
Launchbar::Launchbar()
{
int i;
for(i=0; i<3; i++)
_launchbar_mount[i] = _holdback_mount[i] = _force[i] = 0;
_launchbar_mount[i] = _holdback_mount[i] = _launchbar_force[i]
= _holdback_force[i] = 0;
for(i=0; i<2; i++)
_global_ground[i] = 0;
_global_ground[2] = 1;
@ -22,11 +26,13 @@ Launchbar::Launchbar()
_holdback_length = 2.0;
_down_ang = 0.0;
_up_ang = 0.0;
_ang = 0.0;
_extension = 0.0;
_frac = 0.0;
_frac = _h_frac =0.0;
_launch_cmd = false;
_pos_on_cat = 0.0;
_state = Unmounted;
_strop = false;
}
void Launchbar::setLaunchbarMount(float* position)
@ -46,6 +52,11 @@ void Launchbar::setLength(float length)
_length = length;
}
void Launchbar::setHoldbackLength(float length)
{
_holdback_length = length;
}
void Launchbar::setDownAngle(float ang)
{
_down_ang = ang;
@ -78,12 +89,27 @@ void Launchbar::getLaunchbarMount(float* out)
for(i=0; i<3; i++) out[i] = _launchbar_mount[i];
}
float Launchbar::getLaunchbarPos(int i)
{
return _launchbar_mount[i];
}
void Launchbar::getHoldbackMount(float* out)
{
int i;
for(i=0; i<3; i++) out[i] = _holdback_mount[i];
}
float Launchbar::getHoldbackPos(int j)
{
return _holdback_mount[j];
}
float Launchbar::getHoldbackLength(void)
{
return _holdback_length;
}
float Launchbar::getLength(void)
{
return _length;
@ -99,15 +125,46 @@ float Launchbar::getUpAngle(void)
return _up_ang;
}
float Launchbar::getAngle(void)
{
return _ang;
}
float Launchbar::getHoldbackAngle(void)
{
return _h_ang;
}
float Launchbar::getExtension(void)
{
return _extension;
}
void Launchbar::getForce(float* force, float* off)
void Launchbar::getForce(float* force1, float* off1,
float* force2, float* off2)
{
Math::set3(_launchbar_force, force1);
Math::set3(_launchbar_mount, off1);
Math::set3(_holdback_force, force2);
Math::set3(_holdback_mount, off2);
}
const char* Launchbar::getState(void)
{
switch (_state) {
case Arrested:
return "Engaged";
case Launch:
return "Launching";
case Completed:
return "Completed";
default:
return "Disengaged";
}
}
bool Launchbar::getStrop(void)
{
Math::set3(_force, force);
Math::set3(_launchbar_mount, off);
return _strop;
}
float Launchbar::getCompressFraction()
@ -115,14 +172,43 @@ float Launchbar::getCompressFraction()
return _frac;
}
float Launchbar::getHoldbackCompressFraction()
{
return _h_frac;
}
void Launchbar::getTipPosition(float* out)
{
// The launchbar tip in local coordinates.
float ang = _frac*(_down_ang - _up_ang) + _up_ang;
float pos_tip[3] = { _length*Math::cos(ang), 0.0,-_length*Math::sin(ang) };
Math::add3(_launchbar_mount, pos_tip, out);
_ang = _frac*(_down_ang - _up_ang ) + _up_ang ;
float ptip[3] = { _length*Math::cos(_ang), 0, -_length*Math::sin(_ang) };
Math::add3(_launchbar_mount, ptip, out);
}
float Launchbar::getTipPos(int i)
{
float pos_tip[3];
getTipPosition(pos_tip);
return pos_tip[i];
}
void Launchbar::getHoldbackTipPosition(float* out)
{
// The holdback tip in local coordinates.
_h_ang = _h_frac*(_down_ang - _up_ang) + _up_ang;
float htip[3] = { -_length*Math::cos(_h_ang), 0, -_length*Math::sin(_h_ang) };
Math::add3(_holdback_mount, htip, out);
}
float Launchbar::getHoldbackTipPos(int i)
{
float pos_tip[3];
getHoldbackTipPosition(pos_tip);
return pos_tip[i];
}
void Launchbar::getTipGlobalPosition(State* s, double* out)
{
// The launchbar tip in local coordinates.
@ -168,7 +254,8 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
{
// Init the return values
int i;
for(i=0; i<3; i++) _force[i] = 0;
for(i=0; i<3; i++) _launchbar_force[i] = 0;
for(i=0; i<3; i++) _holdback_force[i] = 0;
if (_state != Unmounted)
_extension = 1;
@ -177,32 +264,91 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
if(_extension <= 0)
return;
if (_extension < _frac)
_frac = _extension;
// For the first guess, the position fraction is equal to the
// extension value.
_frac = _h_frac = _extension;
// The ground plane transformed to the local frame.
float ground[4];
s->planeGlobalToLocal(_global_ground, ground);
// The launchbar tip in local coordinates.
float ltip[3];
getTipPosition(ltip);
// Correct the extension value for no intersection.
// Check if the tip will intersect the ground or not. That is, compute
// the distance of the tip to the ground plane.
float tipdist = ground[3] - Math::dot3(ltip, ground);
if(0 <= tipdist) {
_frac = _extension;
} else {
// Compute the distance of the launchbar mount point from the
// ground plane.
float mountdist = ground[3] - Math::dot3(_launchbar_mount, ground);
// Compute the distance of the launchbar mount point from the
// ground plane in the x-z plane. It holds:
// mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
// thus
float mountdist_xz = _length;
if (ground[2] != 0) {
float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
mountdist_xz = -mountdist*nrm_yz/ground[2];
}
if (mountdist_xz < _length) {
// the launchbar points forward, so we need to change the signs here
float ang = -Math::asin(mountdist_xz/_length)
+ Math::atan2(ground[2], ground[0]) + YASIM_PI2;
ang = -ang;
_frac = (ang - _up_ang)/(_down_ang - _up_ang);
} else {
_frac = _extension;
}
}
// Now do it again for the holdback
// The holdback tip in local coordinates.
float htip[3];
getHoldbackTipPosition(htip);
// Check if the tip will intersect the ground or not. That is, compute
// the distance of the tip to the ground plane.
float h_tipdist = ground[3] - Math::dot3(htip, ground);
if (0 <= h_tipdist) {
_h_frac = _extension;
} else {
// Compute the distance of the holdback mount point from the ground
// plane.
float h_mountdist = ground[3] - Math::dot3(_holdback_mount, ground);
// Compute the distance of the holdback mount point from the ground
// plane in the x-z plane. It holds:
// mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
// thus
float h_mountdist_xz = _holdback_length;
if (ground[2] != 0) {
float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
h_mountdist_xz = -h_mountdist*nrm_yz/ground[2];
}
if (h_mountdist_xz < _holdback_length) {
float h_ang = Math::asin(h_mountdist_xz/_holdback_length)
+ Math::atan2(ground[2], ground[0]) + YASIM_PI2;
_h_frac = (h_ang - _up_ang)/(_down_ang - _up_ang);
} else {
_h_frac = _extension;
}
}
float llb_mount[3];
getTipPosition(llb_mount);
// The launchbar tip in global coordinates.
double launchbar_pos[3];
getTipGlobalPosition(s, launchbar_pos);
// If the launchbars tip is less extended than it could be.
if(_frac < _extension) {
// Correct the extension value for no intersection.
// Compute the distance of the mount point from the ground plane.
double a = - _global_ground[3] + launchbar_pos[0]*_global_ground[0]
+ launchbar_pos[1]*_global_ground[1]
+ launchbar_pos[2]*_global_ground[2];
if(a < _length) {
float ang = Math::asin(a/_length);
_frac = (ang - _up_ang)/(_down_ang - _up_ang);
} else
// FIXME: this will jump
_frac = _extension;
}
// Recompute the launchbar tip.
float llb_mount[3];
getTipPosition(llb_mount);
// The launchbar tip in global coordinates.
s->posLocalToGlobal(llb_mount, launchbar_pos);
double end[2][3]; float vel[2][3];
@ -213,7 +359,7 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
return;
// Compute the positions of the catapult start and endpoints in the
// local coordiante system
// local coordinate system
float lend[2][3];
s->posGlobalToLocal(end[0], lend[0]);
s->posGlobalToLocal(end[1], lend[1]);
@ -237,8 +383,8 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
Math::mul3(1.0/lblen, llbdir, llbdir);
// Check if we are near enough to the cat.
if (_state == Unmounted && dist < 0.5) {
// croase approximation for the velocity of the launchbar.
if (_state == Unmounted && dist < 0.6) {
// coarse approximation for the velocity of the launchbar.
// Might be sufficient because arresting at the cat makes only
// sense when the aircraft does not rotate much.
float lv_mount[3];
@ -258,12 +404,16 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
double dd[2][3]; float fd[2][3]; double ghldbkpos[3];
s->posLocalToGlobal(_holdback_mount, ghldbkpos);
float hbdist = g_cb->getCatapult(ghldbkpos, dd, fd);
float offset = -Math::sqrt(_holdback_length*_holdback_length - hbdist*hbdist);
_pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
// don't let the calculation go -ve here
if (_holdback_length*_holdback_length - hbdist*hbdist < 0)
return;
float offset = -Math::sqrt(_holdback_length*_holdback_length
- hbdist*hbdist);
_pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
// We cannot arrest if we are not at the start of the cat.
if (_pos_on_cat < 0.0 || 0.2 < _pos_on_cat)
if (_pos_on_cat < 0.0 || 0.4 < _pos_on_cat)
return;
// Now we are arrested at the cat.
@ -284,13 +434,13 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
if (_state == Arrested) {
// Now apply a constant tension from the catapult over the launchbar.
Math::mul3(2.0, llbdir, _force);
Math::mul3(2.0, llbdir, _launchbar_force);
// If the distance from the holdback mount at the aircraft to the
// holdback mount on the cat is larger than the holdback length itself,
// the holdback applies a force to the gear.
if (_holdback_length < hldbklen) {
// croase approximation for the velocity of the holdback mount
// coarse approximation for the velocity of the holdback mount
// at the gear.
// Might be sufficient because arresting at the cat makes only
// sense when the aircraft does not rotate much.
@ -308,29 +458,43 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl
// The spring force the holdback will apply to the gear
float tmp[3];
Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir, tmp);
Math::add3(tmp, _force, _force);
Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir,
_holdback_force);
// The damping force here ...
Math::mul3(2e0, lvhldbk, tmp);
Math::sub3(_force, tmp, _force);
Math::sub3(_holdback_force, tmp, _holdback_force);
}
if (_launch_cmd)
if (_launch_cmd) {
_state = Launch;
_strop = false;
}
}
if (_state == Launch) {
// Now apply a constant tension from the catapult over the launchbar.
Math::mul3(25.0, llbdir, _force);
Math::mul3(25.0, llbdir, _launchbar_force);
if (1.0 < dist)
if (1.0 < dist) {
_state = Completed;
}
}
if (_state == Completed) {
// Wait until the strop has cleared the deck
// This is a temporary fix until we come up with something better
if (_frac > 0.8) {
_state = Unmounted;
_strop = true;
}
}
// Scale by the mass. That keeps the stiffness in reasonable bounds.
float mass = body->getTotalMass();
Math::mul3(mass, _force, _force);
Math::mul3(mass, _launchbar_force, _launchbar_force);
Math::mul3(mass, _holdback_force, _holdback_force);
}
}; // namespace yasim

View file

@ -14,7 +14,7 @@ struct State;
//
class Launchbar {
public:
enum LBState { Arrested, Launch, Unmounted };
enum LBState { Arrested, Launch, Unmounted, Completed };
Launchbar();
@ -22,6 +22,7 @@ public:
void setLaunchbarMount(float* position);
void setHoldbackMount(float* position);
void setLength(float length);
void setHoldbackLength(float length);
void setDownAngle(float ang);
void setUpAngle(float ang);
void setExtension(float extension);
@ -30,12 +31,18 @@ public:
void getLaunchbarMount(float* out);
void getHoldbackMount(float* out);
const char* getState(void);
float getLength(void);
float getHoldbackLength(void);
float getDownAngle(void);
float getUpAngle(void);
float getExtension(void);
bool getStrop(void);
void getTipPosition(float* out);
void getHoldbackTipPosition(float* out);
float getTipPos(int i);
float getHoldbackTipPos(int i);
void getTipGlobalPosition(State* s, double* out);
float getPercentPosOnCat(float* lpos, float off, float lends[2][3]);
@ -46,12 +53,18 @@ public:
// vector, and a ground plane (all specified in local coordinates)
// and make a force and point of application (i.e. ground contact)
// available via getForce().
void calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot);
void calcForce(Ground *g_cb, RigidBody* body, State* s,
float* lv, float* lrot);
// Computed values: total force, weight-on-wheels (force normal to
// ground) and compression fraction.
void getForce(float* force, float* off);
void getForce(float* force1, float* off1, float* force2, float* off2);
float getCompressFraction(void);
float getHoldbackCompressFraction(void);
float getAngle(void);
float getHoldbackAngle(void);
float getLaunchbarPos(int i);
float getHoldbackPos(int j);
private:
float _launchbar_mount[3];
@ -60,11 +73,16 @@ private:
float _holdback_length;
float _down_ang;
float _up_ang;
float _ang;
float _h_ang;
float _extension;
float _force[3];
float _launchbar_force[3];
float _holdback_force[3];
float _frac;
float _h_frac;
float _pos_on_cat;
bool _launch_cmd;
bool _strop;
double _global_ground[4];
LBState _state;
};

View file

@ -480,9 +480,10 @@ void Model::calcForces(State* s)
if(_launchbar) {
float v[3], rot[3], glvel[3], ground[3];
_launchbar->calcForce(_ground_cb, &_body, s, lv, lrot);
float force[3], contact[3];
_launchbar->getForce(force, contact);
_body.addForce(contact, force);
float forcelb[3], contactlb[3], forcehb[3], contacthb[3];
_launchbar->getForce(forcelb, contactlb, forcehb, contacthb);
_body.addForce(contactlb, forcelb);
_body.addForce(contacthb, forcehb);
}
}

View file

@ -480,5 +480,9 @@ void YASim::copyFromYASim()
if(l) {
SGPropertyNode * node = fgGetNode("gear/launchbar", 0, true);
node->setFloatValue("position-norm", l->getCompressFraction());
node->setFloatValue("holdback-position-norm", l->getHoldbackCompressFraction());
node->setStringValue("state", l->getState());
node->setBoolValue("strop", l->getStrop());
}
}