// Try the media types of |aEnum| when connecting to |aPin| with type limiter
// of |aFilter|. If |aFilter| is specified, we will only attempt to connect
// with media types from |aEnum| which partially match |aFilter|.
HRESULT
BasePin::TryMediaTypes(IPin *aPin,
const MediaType *aFilter,
IEnumMediaTypes *aEnum)
{
// Reset enumerator.
HRESULT hr = aEnum->Reset();
if (FAILED(hr))
return hr;
while (true) {
AM_MEDIA_TYPE* amt = NULL;
ULONG mediaCount = 0;
HRESULT hr = aEnum->Next(1, &amt, &mediaCount);
if (hr != S_OK) // No more media types...
return VFW_E_NO_ACCEPTABLE_TYPES;
assert(mediaCount == 1 && hr == S_OK);
MediaType* mediaType = reinterpret_cast<MediaType*>(amt);
if (!aFilter || mediaType->MatchesPartial(aFilter)) {
// Either there's no limiter, or we partially match it,
// attempt connection.
if (SUCCEEDED(AttemptConnection(aPin, mediaType)))
return S_OK;
}
}
}
ErrorCode BasePinImpl::AgreeMediaType(IPin *pReceivePin, MediaType* mt)
{
ErrorCode hr = Error; // Assume failure at first
// TODO
#if 0
if (mt && FALSE) // The mediatype is fully specified
{
ErrorCode hr = AttemptConnection(pReceivePin, mt);
if (FAILED(hr)) return hr;
}
#endif
// Try the input pins preferred types
IEnumMediaTypes* pEnumIn = pReceivePin->EnumMediaTypes();
if (pEnumIn)
{
hr = TryMediaTypes(pReceivePin, mt, pEnumIn);
if (hr >= 0)
return hr;
}
// Try the output pins preferred types
IEnumMediaTypes* pEnumOut = EnumMediaTypes();
if (pEnumOut)
{
hr = TryMediaTypes(pReceivePin, mt, pEnumOut);
if (hr >= 0)
return hr;
}
return hr;
}
ErrorCode BasePinImpl::TryMediaTypes(IPin *pReceivePin, MediaType* mt, IEnumMediaTypes *pEnum)
{
MediaType* mt2;//(TRUE);
while (pEnum->Next(1, &mt2/*.GetAddress()*/) == 1)
{
/*
If the pmt parameter is non-NULL, the pin skips media types that do not
match this type. The pmt parameter can specify a partial media type
*/
// TODO, improve this
bool bSkip = false;
#if 1
if (mt)
#endif
{
if (mt->GetMajorType() != GUID_NULL)
{
if (mt2->GetMajorType() != mt->GetMajorType())
bSkip = true;
}
}
if (!bSkip)
{
ErrorCode hr = AttemptConnection(pReceivePin, mt2);
if (hr >= 0) return hr;
}
}
return Error;//E_FAIL;
}
// This pin attempts to connect to |aPin| with media type |aMediaType|.
// If |aMediaType| is fully specified, we must attempt to connect with
// that, else we just enumerate our types, then the other pin's type and
// try them, filtering them using |aMediaType| if it's paritally
// specificed. Used by Connect().
STDMETHODIMP
BasePin::Connect(IPin * aPin,
const AM_MEDIA_TYPE* aMediaType)
{
if (!aPin)
return E_POINTER;
CriticalSectionAutoEnter monitor(*mLock);
if (IsConnected())
return VFW_E_ALREADY_CONNECTED;
// Can't connect when filter is not stopped.
if (!IsStopped())
return VFW_E_NOT_STOPPED;
// Find a muatually acceptable media type. First try the media type
// suggested, then try our media types, then the other pin's media types.
const MediaType* mediaType = reinterpret_cast<const MediaType*>(aMediaType);
if (aMediaType && !mediaType->IsPartiallySpecified()) {
// Media type is supplied and not partially specified, we must try to
// connect with that.
return AttemptConnection(aPin, mediaType);
}
// Try this pin's media types...
IEnumMediaTypesPtr enumMediaTypes;
HRESULT hr = EnumMediaTypes(&enumMediaTypes);
assert(SUCCEEDED(hr));
if (enumMediaTypes) {
hr = TryMediaTypes(aPin, mediaType, enumMediaTypes);
if (SUCCEEDED(hr))
return S_OK;
}
// Can't connect with our media types, try other pins types...
enumMediaTypes = NULL;
hr = aPin->EnumMediaTypes(&enumMediaTypes);
assert(SUCCEEDED(hr));
if (enumMediaTypes) {
hr = TryMediaTypes(aPin, mediaType, enumMediaTypes);
if (SUCCEEDED(hr))
return S_OK;
}
// Nothing connects.
return VFW_E_NO_ACCEPTABLE_TYPES;
}
HRESULT CBasePin::AgreeMediaType(
IPin *pReceivePin,
const CMediaType *pmt)
{
ASSERT(pReceivePin);
IEnumMediaTypes *pEnumMediaTypes = NULL;
// if the media type is fully specified then use that
if ( (pmt != NULL) && (!pmt->IsPartiallySpecified())) {
// if this media type fails, then we must fail the connection
// since if pmt is nonnull we are only allowed to connect
// using a type that matches it.
return AttemptConnection(pReceivePin, pmt);
}
/* Try the other pin's enumerator */
HRESULT hrFailure = VFW_E_NO_ACCEPTABLE_TYPES;
for (int i = 0; i < 2; i++) {
HRESULT hr;
if (i == (int)m_bTryMyTypesFirst) {
hr = pReceivePin->EnumMediaTypes(&pEnumMediaTypes);
} else {
hr = EnumMediaTypes(&pEnumMediaTypes);
}
if (SUCCEEDED(hr)) {
ASSERT(pEnumMediaTypes);
hr = TryMediaTypes(pReceivePin,pmt,pEnumMediaTypes);
pEnumMediaTypes->Release();
if (SUCCEEDED(hr)) {
return NOERROR;
} else {
// try to remember specific error codes if there are any
if ((hr != E_FAIL) &&
(hr != E_INVALIDARG) &&
(hr != VFW_E_TYPE_NOT_ACCEPTED)) {
hrFailure = hr;
}
}
}
}
return hrFailure;
}
void TCPCmd::Exec(std::string str)
{
bool success = false;
bool stop = false;
int argc = 0;
char ** argv = NULL;
int mode = 0; // 0=default, 1=config, 2=connect, 3=stop
Context* ct = Context::Get();
if( ct == NULL )
{
NTerminal::Get()->PrintToStdout("No context found");
return; // no context
}
// parse the arguments
g_shell_parse_argv( str.c_str(), &argc, &argv, NULL );
// must reset optind to 0 in case of previous call
// this resets the arg list and the argv index
optind = 0;
if( argc > 0 )
{
// find the mode
if( strcmp(argv[0],"config") == 0)
{
mode = 1; // config mode
}
else if( strcmp(argv[0], "connect") == 0 )
{
mode = 2; // connect
}
else if( strcmp(argv[0], "stop") == 0 )
{
mode = 3; // stop
stop = true;
}
else if( strcmp(argv[0], "send") == 0 )
{
mode = 4; // send
}
else
{
mode = 0; // default
}
int c;
switch(mode)
{
case 1: // config
while ( (c = getopt(argc, argv, "p:a:vq") ) != -1)
{
switch (c)
{
case 'p':
sscanf(optarg, "%d", &ct->m_DPort);
break;
case 'a':
inet_aton(optarg, &ct->m_Sa.sin_addr);
break;
case 'v':
ct->m_TCPThread->m_Verbose = true;
break;
case 'q':
ct->m_TCPThread->m_Verbose = false;
break;
case '?':
default :
//NTerminal::Get()->PrintToStdout("Unknown argument ");
break;
}
}
break;
case 2: // connect
while ( (c = getopt(argc, argv, "s") ) != -1)
{
switch (c)
{
case 's':
stop = true;
break;
case '?':
default :
NTerminal::Get()->PrintToStdout("Unknown argument ");
break;
}
}
break;
case 4:
if( !ct->m_TCPThread->m_Connected )
{
success = false;
}
else
{
success = true;
// we will expect the string to be in the form
// XX XX XX XX, where XX is an 8 byte hex number
// they will be separated by spaces
std::list<char> data;
int tdata;
for( int i = 1; i < argc; i++ )
{
tdata = strtoul(argv[i], NULL, 16);
data.push_back((char)(tdata&0x000000FF));
}
// lets provide feedback
char* sdata = new char[data.size()*3 +1];
sdata[data.size()*3] = 0; // null terminate it
char tsdat[3];
int k = 0;
for( std::list<char>::iterator it = data.begin();
it != data.end(); it++ )
{
int a = (*it);
a = a&0x000000FF;
sprintf(tsdat, "%02X", a);
sdata[k*3] = tsdat[0];
sdata[k*3+1] = tsdat[1];
sdata[k*3+2] = ' ';
k++;
}
NTerminal::Get()->PrintToStdout("Sending: ");
NTerminal::Get()->PrintToStdout(sdata);
delete [] sdata;
// now we allocate the stream to send
// allocate the data stream
char * stream = new char[data.size()];
k = 0;
for( std::list<char>::iterator it = data.begin();
it != data.end(); it++ )
{
stream[k] = (*it);
k++;
}
// create a message for this one
Message msg;
msg.m_Data = stream;
msg.m_Len = data.size();
// add the message to the outbox
ct->m_TTYThread->AddToOutbox(msg);
//int fd = ct->m_TCPThread->m_Sockfd;
//size_t sent = send( fd, stream, data.size(), 0 );
//if( sent != data.size() )
//{
// NTerminal::Get()->PrintToStdout("Error sending");
//}
// clean up the stream
//delete [] stream;
}
break;
case 0:
case 3:
default:
// do nothing
break;
}
}
else
{
// default mode
mode = 0;
}
g_strfreev(argv);
// work based on mode
switch( mode )
{
case 1: // config
{
char portBuff[10];
sprintf(portBuff, "%d", ct->m_DPort);
// print the configuration
std::string ret = _S("---- TCP CONFIGURATION ------\n") +
_S(" Port: ") + _S(portBuff) + _S("\n") +
_S(" IP : ") + inet_ntoa(ct->m_Sa.sin_addr) + _S("\n") +
_S("-----------------------------\n");
NTerminal::Get()->PrintToStdout(ret);
}
break;
case 2: // connect
case 3: // stop
if( stop )
{
NTerminal::Get()->PrintToStdout("Stopping TCP Connection");
// stop
success = ConnectToServer(*Context::Get(), Context::Get()->m_TCPThread, true);
}
else
{
NTerminal::Get()->PrintToStdout("Starting TCP Connection");
// retry or connect
success = ConnectToServer(*Context::Get(), Context::Get()->m_TCPThread, false);
success &= AttemptConnection(*Context::Get(), Context::Get()->m_TCPThread);
}
case 4: // send
if( success )
{
NTerminal::Get()->PrintToStdout("[Success]");
}
else
{
NTerminal::Get()->PrintToStdout("[Failure]");
}
break;
case 0: // default
default:
if( ct->m_TCPThread->m_Connected )
{
NTerminal::Get()->PrintToStdout("TCP Status = Connected");
}
else
{
NTerminal::Get()->PrintToStdout("TCP Status = Disconnected");
}
break;
}
}
float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void* refcon)
{
(void)elapsedSim;
(void)counter;
(void)refcon;
pendingElapsedTime += elapsedMe;
ReceiveFromComPort();
if (pendingElapsedTime < 0.025) // Don't run faster than 40Hz
{
return -1;
}
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
float gravity_acceleration_x, gravity_acceleration_y, gravity_acceleration_z;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI;
Q_flight = XPLMGetDataf(drQ) / 180 * PI;
R_flight = XPLMGetDataf(drR) / 180 * PI;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
// On 25th Jan 2015, Bill Premerlani confirmed with Austin Meyer, author of X-Plane
// that P, Q and R are rotations in the body frame. So they do not need to be rotated into
// any other frame of reference, other than a small sign correction for the UDB frame conventions.
// Austin Meyer said: "now, i CAN say that P is roll, Q is pitch, and R is yaw, all in degrees per second
//about the aircraft axis,..... (i just looked at the code to confirm this)"
P_plane = P_flight;
Q_plane = -Q_flight; // convert from NED to UDB
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi = (float)((XPLMGetDataf(drPhi) / 180) * PI);
theta = (float)((XPLMGetDataf(drTheta) / 180) * PI);
psi = (float)((XPLMGetDataf(drPsi) / 180) * PI);
// set up a vertical reference for the plotting computations
// vertical in earth frame:
ax = 0;
ay = -(float)9.8;
az = 0;
// get the acceleration loading (gravity-acceleration) in the body frame in "g"s,
// and convert to meter/sec/sec
// x, y, and z are "UDB" coordinates, x is left wing, y is forward, and z is down.
gravity_acceleration_x = (float)((XPLMGetDataf(drg_side)) * 9.8);
gravity_acceleration_y = (float)((XPLMGetDataf(drg_axil)) * 9.8);
gravity_acceleration_z = (float)((XPLMGetDataf(drg_nrml)) * 9.8);
// Convert from OGL frame to Aircraft body fixed frame
// This produces a vertical reference in body frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = -ay; // convert from NED to UDB
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(gravity_acceleration_x * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(gravity_acceleration_y * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(gravity_acceleration_z * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES), NAV_BODYRATES);
while (pendingElapsedTime >= 0.025) // Don't run slower than 40Hz
{
GPSCount++;
if (!IsConnected())
{
ConnectionCount++;
if (ConnectionCount % 160 == 0) // attempt reconnection every 4 seconds when disconnected
{
AttemptConnection();
ConnectionCount = 0;
}
}
if (GPSCount % 10 == 0)
{
GetGPSData();
GPSCount = 0;
}
pendingElapsedTime -= (float)0.025;
}
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings, 0, 8);
static float prevBrakeSetting = PARKBRAKE_ON;
if (BrakeSetting != prevBrakeSetting)
{
prevBrakeSetting = BrakeSetting;
XPLMSetDataf(drBrake, BrakeSetting);
// LoggingFile.mLogFile << "Set parkbrake to " << BrakeSetting << endl;
}
return -1; // get called back on every frame
}
HRESULT CBasePin::TryMediaTypes(
IPin *pReceivePin,
const CMediaType *pmt,
IEnumMediaTypes *pEnum)
{
/* Reset the current enumerator position */
HRESULT hr = pEnum->Reset();
if (FAILED(hr)) {
return hr;
}
CMediaType *pMediaType = NULL;
ULONG ulMediaCount = 0;
// attempt to remember a specific error code if there is one
HRESULT hrFailure = S_OK;
for (;;) {
/* Retrieve the next media type NOTE each time round the loop the
enumerator interface will allocate another AM_MEDIA_TYPE structure
If we are successful then we copy it into our output object, if
not then we must delete the memory allocated before returning */
hr = pEnum->Next(1, (AM_MEDIA_TYPE**)&pMediaType,&ulMediaCount);
if (hr != S_OK) {
if (S_OK == hrFailure) {
hrFailure = VFW_E_NO_ACCEPTABLE_TYPES;
}
return hrFailure;
}
ASSERT(ulMediaCount == 1);
ASSERT(pMediaType);
// check that this matches the partial type (if any)
if ((pmt == NULL) ||
pMediaType->MatchesPartial(pmt)) {
hr = AttemptConnection(pReceivePin, pMediaType);
// attempt to remember a specific error code
if (FAILED(hr) &&
SUCCEEDED(hrFailure) &&
(hr != E_FAIL) &&
(hr != E_INVALIDARG) &&
(hr != VFW_E_TYPE_NOT_ACCEPTED)) {
hrFailure = hr;
}
} else {
hr = VFW_E_NO_ACCEPTABLE_TYPES;
}
DeleteMediaType(pMediaType);
if (S_OK == hr) {
return hr;
}
}
}
