// 現在のコールスタックの取得
CALLSTACK_DATA *GetCallStack()
{
CALLSTACK_DATA *s;
if (do_not_get_callstack)
{
// コールスタックは取得しない
return NULL;
}
OSLock(cs_lock);
{
// コールスタックの取得
s = OSGetCallStack();
}
OSUnlock(cs_lock);
if (s == NULL)
{
return NULL;
}
// コールスタックを 3 つ分だけ下降する
s = WalkDownCallStack(s, 3);
return s;
}
STATUS NF_Command(char *server ,char* szcommand,char * szResult)
{
HANDLE hRetInfo;
STATUS nError;
char *pBuffer;
nError = NotesInitExtended (0, NULL);
if (nError)
return ERR(nError);
nError = NSFRemoteConsole (server, szcommand, &hRetInfo);
if (nError != NOERROR)
{
OSLoadString(NULLHANDLE, ERR(nError), szResult, 1024);
CutFlag(szResult,0x13);
NotesTerm();
return (ERR(nError));
}
pBuffer = OSLock (char, hRetInfo);
strcpy(szResult,pBuffer);
OSUnlock (hRetInfo);
OSMemFree (hRetInfo);
NotesTerm();
return (NOERROR);
}
// Get the current call stack
CALLSTACK_DATA *GetCallStack()
{
CALLSTACK_DATA *s;
if (do_not_get_callstack)
{
// Not to get the call stack
return NULL;
}
OSLock(cs_lock);
{
// Get the call stack
s = OSGetCallStack();
}
OSUnlock(cs_lock);
if (s == NULL)
{
return NULL;
}
// Descend in the call stack for 3 steps
s = WalkDownCallStack(s, 3);
return s;
}
// Unlock
void UnlockInner(LOCK *lock)
{
// Validate arguments
if (lock == NULL)
{
return;
}
// KS
KS_INC(KS_UNLOCK_COUNT);
KS_DEC(KS_CURRENT_LOCKED_COUNT);
OSUnlock(lock);
}
// コールスタックのシンボル情報を取得
bool GetCallStackSymbolInfo(CALLSTACK_DATA *s)
{
bool ret;
// 引数チェック
if (s == NULL)
{
return false;
}
OSLock(cs_lock);
{
ret = OSGetCallStackSymbolInfo(s);
}
OSUnlock(cs_lock);
return ret;
}
// 新しいオブジェクトを追跡リストに入れる
void TrackNewObj(UINT64 addr, char *name, UINT size)
{
TRACKING_OBJECT *o;
UINT new_id;
// 引数チェック
if (addr == 0 || name == NULL)
{
return;
}
if (IsMemCheck() == false)
{
// リリースモードでは追跡しない
return;
}
if (disable_tracking)
{
return;
}
// 新しい ID の生成
OSLock(obj_id_lock);
{
new_id = ++obj_id;
}
OSUnlock(obj_id_lock);
o = OSMemoryAlloc(sizeof(TRACKING_OBJECT));
o->Id = new_id;
o->Address = addr;
o->Name = name;
o->Size = size;
o->CreatedDate = LocalTime64();
o->CallStack = WalkDownCallStack(GetCallStack(), 2);
o->FileName[0] = 0;
o->LineNumber = 0;
LockTrackingList();
{
InsertTrackingList(o);
}
UnlockTrackingList();
}
// Put a new object into the tracking list
void TrackNewObj(UINT64 addr, char *name, UINT size)
{
TRACKING_OBJECT *o;
UINT new_id;
// Validate arguments
if (addr == 0 || name == NULL)
{
return;
}
if (IsMemCheck() == false)
{
// Don't track in the release mode
return;
}
if (disable_tracking)
{
return;
}
// Generate a new ID
OSLock(obj_id_lock);
{
new_id = ++obj_id;
}
OSUnlock(obj_id_lock);
o = OSMemoryAlloc(sizeof(TRACKING_OBJECT));
o->Id = new_id;
o->Address = addr;
o->Name = name;
o->Size = size;
o->CreatedDate = LocalTime64();
o->CallStack = WalkDownCallStack(GetCallStack(), 2);
o->FileName[0] = 0;
o->LineNumber = 0;
LockTrackingList();
{
InsertTrackingList(o);
}
UnlockTrackingList();
}
void IMU_Task(void * p)
{
OS_SEM MySem;
OSSemInit(&MySem,0);
PiterSem(&MySem, 200);
while(1)
{
OSSemPend(&MySem,0);
if ((Local_IMU_Result.ReadingNum %10)==0)
ImuRead(Local_IMU_Result,true);
else
ImuRead(Local_IMU_Result,false);
Local_IMU_Result.ReadingNum++;
OSLock();
memcpy((void*)&IMU_Result,&Local_IMU_Result,sizeof(IMU_Result));
OSUnlock();
if(LocalSem) OSSemPost(LocalSem);
}
}
// トラッキングリストのロック解除
void UnlockTrackingList()
{
OSUnlock(obj_lock);
}
/*-------------------------------------------------------------------
* UserMain
*-----------------------------------------------------------------*/
void UserMain(void *pd)
{
SimpleUart(0,115200);
assign_stdio(0);
iprintf("Before init stack\r\n");
InitializeStack();
{
WORD ncounts = 0;
while ( ( !bEtherLink ) && ( ncounts < 2*TICKS_PER_SECOND ) )
{
ncounts++;
OSTimeDly( 1 );
}
}
EnableAutoUpdate();
EnableTaskMonitor();
OSChangePrio( MAIN_PRIO ); // set standard UserMain task priority
//BcastSysLogPrintf("Application built on %s on %s\r\nWait...", __TIME__, __DATE__ );
pUserUdpProcessFunction=MyUDPProcessFunction;
// UdpCheck();
OSSemInit(&DataSem,0);
LoadSensorConfig();
// BcastSysLogPrintf("1");
InitLog();
ImuInit(IMU_PRIO,&DataSem);
InitIDSM2SubSystem(RC_PRIO,&DataSem);
InitGpsSubSystem(GPS_PRIO,&DataSem);
InitServos();
SetServo(0,0);
SetServo(1,0);
SetServo(2,0);
SetServo(3,0);
WORD LastGps=GPS_Result.ReadingNum;
WORD LastRc =DSM2_Result.ReadingNum;
WORD LastImu =IMU_Result.ReadingNum;
DWORD LSecs=Secs;
static short mmx;
static short mmy;
static short nmx;
static short nmy;
static short mgz;
static short ngz;
DWORD ZeroTime=Secs+5;
long GZSum=0;
int GZCnt=0;
float fmh=0.0;
while(ZeroTime > Secs)
{
OSSemPend(&DataSem,20);
if(LastImu !=IMU_Result.ReadingNum)
{
GZSum+=IMU_Result.gz;
GZCnt++;
LastImu =IMU_Result.ReadingNum;
if (IMU_Result.mx !=0 )
fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my);
}
}
short gzoff=(GZSum/GZCnt);
float fIheading=fmh;
while(1)
{
OSSemPend(&DataSem,20);
if (LastGps!=GPS_Result.ReadingNum )
{
LastGps=GPS_Result.ReadingNum;
LogSmGps(GPS_Result);
}
if (LastRc !=DSM2_Result.ReadingNum)
{
WORD w=DSM2_Result.val[1];
float f=DSM_Con(w);
SetServo(STEER_CH,f);
w=DSM2_Result.val[0];
f=DSM_Con(w);
SetServo(THROTTLE_CH,f);
LastRc =DSM2_Result.ReadingNum;
// LogRC(DSM2_Result);
}
if(LSecs!=Secs)
{
// BcastSysLogPrintf("Tick %d Iat:%ld lon:%ld SAT:%d\r\n",Secs,GPS_Result.LAT,GPS_Result.LON,GPS_Result.numSV);
//SysLogPrintf(ipa_syslog_addr,514,
LogMaxMin(mgz,mmx,mmy,ngz,nmx,nmy);
//static char tbuf[256];
// siprintf(tbuf,"TCN=%ld\r\n",sim.timer[0].tcn);
// writestring(LOG_UART,tbuf);
mgz=IMU_Result.mz;
mmx=IMU_Result.mx;
mmy=IMU_Result.my;
ngz=IMU_Result.mz;
nmx=IMU_Result.mx;
nmy=IMU_Result.my;
LSecs=Secs;
}
if(LastImu !=IMU_Result.ReadingNum)
{
if(IMU_Result.gz<ngz) ngz=IMU_Result.gz;
if(IMU_Result.gz>mgz) mgz=IMU_Result.gz;
fIheading+=(fGZHeadingScale_deg*(IMU_Result.gz-gzoff));
if(fIheading > 180.0 ) fIheading-=360.0;
if(fIheading <-180.0 ) fIheading+=360.0;
if (IMU_Result.mx !=0 )
{
if(IMU_Result.mx<nmx) nmx=IMU_Result.mx;
if(IMU_Result.mx>mmx) mmx=IMU_Result.mx;
if(IMU_Result.my<nmy) nmy=IMU_Result.my;
if(IMU_Result.my>mmy) mmy=IMU_Result.my;
volatile ImuRegisters il;
OSLock();
il.ax=IMU_Result.ax;
il.ay=IMU_Result.ay;
il.az=IMU_Result.az;
il.gx=IMU_Result.gx;
il.gy=IMU_Result.gy;
il.gz=IMU_Result.gz;
il.mx=IMU_Result.mx;
il.my=IMU_Result.my;
il.mz=IMU_Result.mz;
il.t =IMU_Result.t ;
il.ReadingNum=IMU_Result.ReadingNum;
OSUnlock();
fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my);
CorrectHeading(fIheading,fmh,0.005);//20 times per second correct in 10 seconds so 0.005
il.fIhead=fIheading;
il.fMhead=fmh;
il.GHeading= GPS_Result.Heading;
il.odo=sim.timer[0].tcn;
LogImu(il);
}
LastImu =IMU_Result.ReadingNum;
}
}//While
}
/******************************************************************************
Takes the fullpath to a Domino directory. Returns the first match
to 'name' from the '($Users)' view.
Allows case-insensitive & partial match
Returns empty string if not found
******************************************************************************/
std::wstring RecipientsHandler::SearchDominoDirectory(char* pszDirectoryPath, const std::wstring &name)
{
std::wstring result;
WORD wABCount = 0;
WORD wABBufferLength = 0;
WORD wSignalFlags;
WORD wItemDataType;
DWORD dwMatches = 0;
DWORD dwEntriesFound = 0;
HANDLE hABBuffer = 0;
HANDLE hSummaryBuffer = 0;
DBHANDLE hdbDirectory = 0;
HCOLLECTION hCollection = 0;
NOTEID nidLookupView;
COLLECTIONPOSITION cp;
ITEM_VALUE_TABLE* pivt;
WORD* pItemLengthTable;
BYTE* pItem;
LIST* plHeader;
char szBuffer[MAXUSERNAME] = {0};
STATUS status = NSFDbOpen(pszDirectoryPath, &hdbDirectory);
if (NOERROR == status)
{
status = NIFFindView(hdbDirectory, USERNAMESSPACE_ALT, &nidLookupView);
if (NOERROR == status)
{
status = NIFOpenCollection( hdbDirectory,
hdbDirectory,
nidLookupView,
0,
NULLHANDLE,
&hCollection,
NULLHANDLE,
NULL,
NULLHANDLE,
NULLHANDLE);
if (NOERROR == status)
{
status = NIFFindByName( hCollection,
Workshare::Conversions::W22LMBCS(name).c_str(),
FIND_PARTIAL | FIND_CASE_INSENSITIVE,
&cp,
&dwMatches);
if (NOERROR == status && dwMatches > 0)
{
do
{
status = NIFReadEntries(hCollection,
&cp,
NAVIGATE_CURRENT,
0,
NAVIGATE_NEXT,
dwMatches,
READ_MASK_SUMMARYVALUES,
&hSummaryBuffer,
NULL,
NULL,
&dwEntriesFound,
&wSignalFlags);
if (NOERROR == status && hSummaryBuffer != NULLHANDLE)
{
pivt = OSLock(ITEM_VALUE_TABLE, hSummaryBuffer);
if (dwEntriesFound > 0)
{
if (pivt->Items > 0)
{
pItemLengthTable = (WORD*)++pivt;
pivt--;
pItem = (BYTE*)pItemLengthTable;
pItem += (pivt->Items * sizeof(WORD));
// at this point pItemLength -> 1st item length
// pItem -> 1st item datatype WORD
wItemDataType = *((WORD*)pItem);
pItem += sizeof(WORD); // pItem -> data
switch (wItemDataType)
{
case TYPE_TEXT:
{
strncpy_s(szBuffer, sizeof(szBuffer), (char*)pItem, pItemLengthTable[0]);
result = Workshare::Conversions::LMBCS22W(szBuffer);
break;
}
case TYPE_TEXT_LIST:
{
plHeader = (LIST*)pItem;
if (plHeader->ListEntries > 0)
{
pItemLengthTable = (WORD*)++plHeader;
plHeader--;
pItem = (BYTE*)plHeader;
pItem += sizeof(LIST) + plHeader->ListEntries * sizeof(WORD);
strncpy_s(szBuffer, sizeof(szBuffer), (char*)pItem, pItemLengthTable[0]);
result = Workshare::Conversions::LMBCS22W(szBuffer);
}
break;
}
default:
{
break;
}
}
}
}
OSUnlock(hSummaryBuffer);
OSMemFree(hSummaryBuffer);
}
wSignalFlags = 0; // to ensure we only go around once
} while (wSignalFlags & SIGNAL_MORE_TO_DO);
}
NIFCloseCollection(hCollection);
}
}
NSFDbClose(hdbDirectory);
}
return result;
}
/******************************************************************************
Searches a CONDENSED directory for the specified name
Allows case-insensitive & partial match
Returns empty string if not found
******************************************************************************/
std::wstring RecipientsHandler::SearchCondensedDirectory(char* pszDirectoryPath, const std::wstring &name)
{
#define LOOKUP_VIEW "Users"
// Column numbers counting from 0
#define SEARCH_COLUMN 1 // lookup column
#define RETURN_COLUMN 3 // return column(@Subset(FullName;1)
// lookup column must be < return column
std::wstring result;
std::wstring columnvalue;
WORD wABCount = 0;
WORD wABBufferLength = 0;
WORD wSignalFlags;
WORD wItemDataType;
WORD wIVTLength;
DWORD dwEntriesFound = 0;
DWORD dwEntryIndex;
HANDLE hABBuffer = 0;
HANDLE hSummaryBuffer = 0;
DBHANDLE hdbDirectory = 0;
HCOLLECTION hCollection = 0;
NOTEID nidLookupView;
COLLECTIONPOSITION cp;
ITEM_VALUE_TABLE* pivt;
BYTE* pSummaryBuffer;
WORD* pItemLengthTable;
BYTE* pItem;
int nColumnCount;
char szBuffer[MAXUSERNAME] = {0};
STATUS status = NSFDbOpen(pszDirectoryPath, &hdbDirectory);
if (NOERROR == status)
{
status = NIFFindView(hdbDirectory, LOOKUP_VIEW, &nidLookupView);
if (NOERROR == status)
{
status = NIFOpenCollection( hdbDirectory,
hdbDirectory,
nidLookupView,
0,
NULLHANDLE,
&hCollection,
NULLHANDLE,
NULL,
NULLHANDLE,
NULLHANDLE);
if (NOERROR == status)
{
cp.Level = 0;
cp.Tumbler[0] = 0;
do
{
status = NIFReadEntries(hCollection,
&cp,
NAVIGATE_NEXT,
1,
NAVIGATE_NEXT,
0xffffffff,
READ_MASK_SUMMARYVALUES,
&hSummaryBuffer,
NULL,
NULL,
&dwEntriesFound,
&wSignalFlags);
if (NOERROR == status && hSummaryBuffer != NULLHANDLE)
{
pSummaryBuffer = OSLock(BYTE, hSummaryBuffer);
wIVTLength = 0;
for (dwEntryIndex = 0; dwEntryIndex < dwEntriesFound; dwEntryIndex++)
{
pSummaryBuffer += wIVTLength;
pivt = (ITEM_VALUE_TABLE*)pSummaryBuffer;
wIVTLength = pivt->Length;
if (pivt->Items > RETURN_COLUMN)
{
pItemLengthTable = (WORD*)++pivt;
pivt--;
pItem = (BYTE*)pItemLengthTable;
pItem += (pivt->Items * sizeof(WORD));
// at this point pItemLengthTable -> 1st item length
// pItem -> 1st item datatype WORD
// bump up to the specified COLUMN_NUMBER
for (nColumnCount = 0; nColumnCount < SEARCH_COLUMN; nColumnCount++)
{
pItem += *pItemLengthTable++;
}
wItemDataType = *((WORD*)pItem);
pItem += sizeof(WORD); // pItem -> data
if (wItemDataType == TYPE_TEXT)
{
strncpy_s(szBuffer, sizeof(szBuffer), (char*)pItem, pItemLengthTable[0] - sizeof(WORD));
columnvalue = Workshare::Conversions::LMBCS22W(szBuffer);
if (_wcsnicmp(columnvalue.c_str(), name.c_str(), name.length()) == 0)
{
pItem -= sizeof(WORD);
// bump up to the column to be returned
for (nColumnCount = SEARCH_COLUMN; nColumnCount < RETURN_COLUMN; nColumnCount++)
{
pItem += *pItemLengthTable++;
}
wItemDataType = *((WORD*)pItem);
pItem += sizeof(WORD); // pItem -> data
if (wItemDataType == TYPE_TEXT)
{
strncpy_s(szBuffer, sizeof(szBuffer), (char*)pItem, pItemLengthTable[0] - sizeof(WORD));
result = Workshare::Conversions::LMBCS22W(szBuffer);
wSignalFlags = 0; // exits do - while
break; // exits for
}
}
}
}
} // end for
OSUnlock(hSummaryBuffer);
OSMemFree(hSummaryBuffer);
}
}
while (wSignalFlags & SIGNAL_MORE_TO_DO);
NIFCloseCollection(hCollection);
}
}
NSFDbClose(hdbDirectory);
}
return result;
}
/******************************************************************************
This function attempts to find the first matching name from the
supplied (possibly) partial name. Searches local, server
or additional directories
******************************************************************************/
std::wstring RecipientsHandler::GetMatchingName(const std::wstring &name, const std::wstring &server, bool exhaustive)
{
std::wstring result;
WORD wEntry;
WORD wABCount;
WORD wABBufferLength;
HANDLE hABBuffer = 0;
char szABServer[MAXPATH];
char szABFileName[MAXPATH];
char szABFullPath[MAXPATH];
//char szCaption[MAXUSERNAME + 64];
//strcpy_s(szCaption, "Searching for ");
//strcat_s(szCaption, sizeof(szCaption) - strlen(szCaption), Workshare::Conversions::W22LMBCS(name).c_str());
//IXPostMessage(szCaption);
STATUS status = NAMEGetAddressBooks(server.empty() ? NULL : Workshare::Conversions::W22LMBCS(server).c_str(),
exhaustive ? 0 : NAME_GET_AB_FIRSTONLY,
&wABCount,
&wABBufferLength,
&hABBuffer);
if (NOERROR != status)
{
LOG_WS_ERROR(L"Error: NAMEGetAddressBooks");
return L"";
}
// if stat was good, but no directories returned, we still have to free the buffer
if (wABCount == 0)
{
if (hABBuffer != 0)
OSMemFree(hABBuffer);
LOG_WS_INFO(L"No directories returned.");
return L"";
}
char* pszABBuffer = OSLock(char, hABBuffer);
for (wEntry = 0; wEntry < wABCount; wEntry++)
{
OSPathNetParse(pszABBuffer, NULL, szABServer, szABFileName);
if (strlen(szABServer) == 0 && !server.empty())
{
strcpy_s(szABServer, sizeof(szABServer), Workshare::Conversions::W22LMBCS(server).c_str());
}
OSPathNetConstruct( NULL, szABServer, szABFileName, szABFullPath);
if (IsCondensedDirectory(szABFullPath))
{
result = SearchCondensedDirectory(szABFullPath, name);
}
else
{
result = SearchDominoDirectory(szABFullPath, name);
}
if (!result.empty())
{
break;
}
pszABBuffer += (strlen(pszABBuffer) + 1);
}
OSUnlock(hABBuffer); // done with this now
OSMemFree(hABBuffer); // unlock and free it
if (result.empty())
{
//strcpy_s(szCaption, sizeof(szCaption), "Could not find ");
//strcat_s(szCaption, sizeof(szCaption) - strlen(szCaption), Workshare::Conversions::W22LMBCS(name).c_str());
//IXPostMessage(szCaption);
std::wostringstream msg;
msg << "No match found: name=[" << name.c_str() << "] server=[" << server.c_str() << "]" << std::ends;
LOG_WS_INFO(msg.str().c_str());
return L"";
}
//strcpy_s(szCaption, sizeof(szCaption), "Found ");
//strcat_s(szCaption, sizeof(szCaption) - strlen(szCaption), Workshare::Conversions::W22LMBCS(result).c_str());
//IXPostMessage(szCaption);
std::wostringstream msg;
msg << "Match found: name=[" << name.c_str() << "] match=[" << result.c_str() << "] server=[" << server.c_str() << "]" << std::ends;
LOG_WS_INFO(msg.str().c_str());
return result;
}
