QString BApplicationPrivate::findImage(const QString &subdir, const QString &name, const QStringList &preferredFormats)
{
if (!testInit("BApplicationPrivate"))
return "";
if (subdir.isEmpty() || name.isEmpty())
return "";
QString bfn = QFileInfo(name).baseName();
QStringList suffixes;
foreach (const QByteArray &ba, QImageReader::supportedImageFormats())
suffixes << QString(ba);
int indsvg = suffixes.indexOf("svg");
if (indsvg >= 0)
suffixes.insert(indsvg, "svgz");
for (int i = preferredFormats.size() - 1; i >= 0; --i) {
const QString &fmt = preferredFormats.at(i);
if (suffixes.contains(fmt)) {
suffixes.removeAll(fmt);
suffixes.prepend(fmt);
}
}
foreach (const QString &suff, suffixes) {
if (suff.isEmpty())
continue;
QString fn = BDirTools::findResource(subdir + "/" + bfn + "." + suff, BDirTools::GlobalOnly);
if (!fn.isEmpty())
return fn;
}
return "";
}
void TestShapeTriangle::test2DPointContained()
{
testInit(__func__);
double width = 200;
double height = 200;
Point origin = Point(100,100,0);
Point top = Point(100, height, 0);
Point right = Point(width, 100, 0);
ShapeTriangle triangle = ShapeTriangle(origin, top, right);
// Tests
// See if all vertices are contained
if (!triangle.pointContained(origin)) {
testFailed();
}
if (!triangle.pointContained(top)) {
testFailed();
}
if (!triangle.pointContained(right)) {
testFailed();
}
// Test point inside
Point inside = Point(120, 120);
if (!triangle.pointContained(inside)) {
testFailed();
}
// Point outside
Point outside = Point(99, 99);
if (triangle.pointContained(outside)) {
testFailed();
}
testInterpret();
}
int main(int argc, char **argv)
{
testInit(argc, argv, 400,400, "ShivaVG: Radial Gradient Test");
testCallback(TEST_CALLBACK_DISPLAY, (CallbackFunc)display);
testCallback(TEST_CALLBACK_BUTTON, (CallbackFunc)click);
testCallback(TEST_CALLBACK_DRAG, (CallbackFunc)drag);
testCallback(TEST_CALLBACK_KEY, (CallbackFunc)key);
p = testCreatePath();
center = testCreatePath();
focus = testCreatePath();
radius = testCreatePath();
cx = testWidth()/2;
cy = testHeight()/2;
fx = cx;
fy = cy;
r = sqx/2;
radialFill = vgCreatePaint();
blackFill = vgCreatePaint();
vgSetParameterfv(blackFill, VG_PAINT_COLOR, 4, black);
createSquare(p);
createRadial();
testOverlayString("Press H for a list of commands");
testOverlayColor(1,1,1,1);
testRun();
return EXIT_SUCCESS;
}
int main( int argc, char *argv[] )
{
time_t shouldend;
testInit( &argc, argv );
getExtensionEntries( );
palBombOnError( );
init( ( const ALbyte * ) ( ( argc == 1 ) ? WAVEFILE : argv[1] ) );
alSourcePlay( movingSource );
shouldend = time( NULL );
while( ( shouldend - start ) <= 10 ) {
iterate( );
shouldend = time( NULL );
if( ( shouldend - start ) > 10 ) {
alSourceStop( movingSource );
}
}
testExit();
return EXIT_SUCCESS;
}
int tiger()
{
int argc;
char **argv;
//Initial windows(glut) and some gl projection
testInit(argc, argv, 320 ,480, "ShivaVG: Tiger SVG Test");
#ifdef USE_GL
//callbacks[type] = func
testCallback(TEST_CALLBACK_CLEANUP, (CallbackFunc)cleanup);
testCallback(TEST_CALLBACK_DISPLAY, (CallbackFunc)display);
testCallback(TEST_CALLBACK_KEY, (CallbackFunc)key);
testCallback(TEST_CALLBACK_BUTTON, (CallbackFunc)click);
testCallback(TEST_CALLBACK_DRAG, (CallbackFunc)drag);
#endif //USE_GL
time_t stime, etime;
double tpp;
double fps;
loadTiger();
display(0.05);
#ifdef USE_GL
testOverlayString("Press H for a list of commands");
testRun();
#endif //USE_GL
return EXIT_SUCCESS;
}
int main(int argc, char **argv)
{
testInit(argc, argv, 400,400, "ShivaVG: Linear Gradient Test");
testCallback(TEST_CALLBACK_DISPLAY, (CallbackFunc)display);
testCallback(TEST_CALLBACK_BUTTON, (CallbackFunc)click);
testCallback(TEST_CALLBACK_DRAG, (CallbackFunc)drag);
testCallback(TEST_CALLBACK_KEY, (CallbackFunc)key);
testCallback(TEST_CALLBACK_RESHAPE, (CallbackFunc)reshape);
p = testCreatePath();
org = testCreatePath();
blackFill = vgCreatePaint();
vgSetParameterfv(blackFill, VG_PAINT_COLOR, 4, black);
backImage = createImageFromJpeg(IMAGE_DIR"test_img_violin.jpg");
patternImage = createImageFromJpeg(IMAGE_DIR"test_img_shivavg.jpg");
patternFill = vgCreatePaint();
createSquare(p);
createOrigin(org);
createPattern();
testOverlayString("Press H for a list of commands");
testOverlayColor(1,1,1,1);
testRun();
return EXIT_SUCCESS;
}
int main()
{
setup();
testInit();
testSendKeyDownEvent();
testSendKeyUpEvent();
testRetrieveSelectedIndex();
testEndButtonClicked();
return 0;
}
int main(int argc, char **argv)
{
testInit(argc, argv, 500,500, "ShivaVG: VGU Primitives Test");
testCallback(TEST_CALLBACK_DISPLAY, (CallbackFunc)display);
createPrimitives();
testRun();
return EXIT_SUCCESS;
}
QAction *BApplicationPrivate::createStandardAction(BGuiTools::StandardAction type, QObject *parent)
{
if (!testInit("BApplicationPrivate"))
return 0;
QAction *act = 0;
switch (type) {
case BGuiTools::SettingsAction:
act = new QAction(parent);
act->setMenuRole(QAction::PreferencesRole);
act->setObjectName("ActionSettings");
act->setIcon(BApplication::icon("configure"));
act->setShortcut(QKeySequence::Preferences);
connect(act, SIGNAL(triggered()), qs_func(), SLOT(showSettingsDialog()));
break;
case BGuiTools::HomepageAction:
act = new QAction(parent);
act->setObjectName("ActionHomepage");
act->setIcon(BApplication::icon("gohome"));
connect(act, SIGNAL(triggered()), qs_func(), SLOT(openHomepage()));
break;
case BGuiTools::HelpContentsAction:
act = new QAction(parent);
act->setObjectName("ActionHelpContents");
act->setIcon(BApplication::beqtIcon("help_contents"));
connect(act, SIGNAL(triggered()), qs_func(), SLOT(showHelpContents()));
break;
case BGuiTools::ContextualHelpAction:
act = new QAction(parent);
act->setObjectName("ActionContextualHelp");
act->setIcon(BApplication::icon("help_contextual"));
connect(act, SIGNAL(triggered()), qs_func(), SLOT(showContextualHelp()));
break;
case BGuiTools::WhatsThisAction:
act = QWhatsThis::createAction(parent);
act->setObjectName("ActionWhatsThis");
act->setIcon(BApplication::beqtIcon("help_hint"));
break;
case BGuiTools::AboutAction:
act = new QAction(parent);
act->setMenuRole(QAction::AboutRole);
act->setObjectName("ActionAbout");
act->setIcon(BApplication::icon("help_about"));
connect(act, SIGNAL(triggered()), qs_func(), SLOT(showAboutDialog()));
break;
default:
return 0;
}
act->setProperty("beqt/standard_action_type", type);
qs_func()->ds_func()->actions.insert(act, act);
connect(act, SIGNAL(destroyed(QObject *)), qs_func()->ds_func(), SLOT(actionDestroyed(QObject *)));
retranslateStandardAction(act);
return act;
}
int main(void) {
halInit();
chSysInit();
testInit();
while (true) {
if (palReadPad(GPIOA, GPIOA_BUTTON))
chThdSleepMilliseconds(500);
}
}
int main(void)
{
RwlockInit( &lock, 1 );
testInit();
testMark();
testGrow();
testGather();
RwlockCleanup( &lock );
return 0;
}
int CXXTest::doTests()
{
LOG_INFO("Running CXX tests");
if (testInit() != TEST_PASS) { FAIL("C++ init test failed"); }
if (testCreate() != TEST_PASS) { FAIL("pthread_create test failed"); }
if (testSelf() != TEST_PASS) { FAIL("pthread_self test failed"); }
if (testExit() != TEST_PASS) { FAIL("pthread_exit test failed"); }
return TEST_PASS;
}
int main(int argc, char** argv)
{
testInit();
/*
CTestIpxKnxPtl ipxKnxPtlTester;
ipxKnxPtlTester.test();
*/
/*
CTestIpKnxApp ipKnxAppTester;
ipKnxAppTester.test();
*/
/*
CTestEasyTcpClt tcpcltTester;
tcpcltTester.test();
while (1)
Sleep(1000);
*/
/*
CTestEHSCmdParser tester;
tester.test();
/*/
/*
CTestRdbManager rdbManagerTester;
rdbManagerTester.test();
*/
//*
CDataPushOutorTester dataPushOutorTester;
dataPushOutorTester.test();
//*/
/*
CTestEhdbManager ehdbManagerTester;
ehdbManagerTester.test();
*/
testDeinit();
return 0;
}
int main()
{
TerminalState *state = new VTTerminalState();
Logger::getInstance()->setLogLevel(Logger::ERROR);
state->enableShiftText(true);
//Populate test data.
for (int i = 0; i < 40; i++)
{
for (int j = 0; j < 80; j++)
{
terminalBuffer[i][j] = (rand() % 95) + 32;
}
}
testInit(state);
testCursor(state);
testOrigin(state);
testCursorMoveBackward(state);
testCursorMoveForward(state);
testCursorMoveUp(state);
testCursorMoveDown(state);
state->addTerminalModeFlags(TS_TM_ORIGIN);
testCursorMoveBackward(state);
testCursorMoveForward(state);
state->removeTerminalModeFlags(TS_TM_ORIGIN);
testCursorMoveUpOrigin(state);
testCursorMoveDownOrigin(state);
testInsert(state);
testExpandedBuffer(state);
testErase(state);
testInsertShift(state);
testDelete(state);
testVT((VTTerminalState *)state);
testGraphicsState();
delete state;
return 0;
}
int main( int argc, char *argv[] )
{
testInit( &argc, argv );
init( ( const ALbyte * ) ( ( argc == 1 ) ? WAVEFILE : argv[1] ) );
start( );
while( sourceIsPlaying( multis ) == AL_TRUE ) {
microSleep( 1000000 );
microSleep( 1000000 );
microSleep( 1000000 );
microSleep( 1000000 );
}
testExit();
return EXIT_SUCCESS;
}
void main()
{
testInit();
std::string init = "a red black tree walks into a bar "
"has johnny walker on the rocks "
"and quickly rebalances itself."
"A RED BLACK TREE WALKS INTO A BAR "
"HAS JOHNNY WALKER ON THE ROCKS "
"AND QUICKLY REBALANCES ITSELF.";
auto t = inserted(RBTree<char>(), init.begin(), init.end());
print(t);
t.assert1();
std::cout << "Black depth: " << t.countB() << std::endl;
std::cout << "Member z: " << t.member('z') << std::endl;
std::for_each(init.begin(), init.end(), [t](char c)
{
if (!t.member(c))
std::cout << "Error: " << c << " not found\n";
});
}
int main(int argc, const char **argv) {
printf("Running all tests...\n\n");
//**** core tests
testTree();
testParse();
testDef();
testWord();
testFlow();
testVM();
testStack();
testInit();
testCommand();
testConversation();
testScape();
// testThreads();
//**** builtins tests
testNoun();
testPattern();
//testArray();
// testStream();
testInt();
testStr255();
testCfunc();
testReceptorUtil();
// testReceptor();
//testVmHost();
//**** examples test
testPoint();
// testLine();
report_tests();
return 0;
}
void TestShapeTriangle::testDeterminant3D()
{
testInit(__func__);
double width = 200;
double height = 200;
Point origin = Point(123,456,0);
Point top = Point(333, height, 0);
Point right = Point(width, 333, 0);
ShapeTriangle triangle = ShapeTriangle(origin, top, right);
// Tests
double a11 = origin.getX() - right.getX();
double a12 = top.getX() - right.getX();
double a21 = origin.getY() - right.getY();
double a22 = top.getY() - right.getY();
double determinant3D = 0;
if (determinant3D != triangle.getDeterminant3D()) {
testFailed();
}
testInterpret();
}
int main(void)
{
/*
* Init the runtime, test and say hello.
*/
RTTEST hTest;
RTEXITCODE rcExit = RTTestInitAndCreate("tstUSBProxyLinux", &hTest);
if (rcExit != RTEXITCODE_SUCCESS)
return rcExit;
RTTestBanner(hTest);
/*
* Run the tests.
*/
testInit(hTest);
testCheckDeviceRoot(hTest);
/*
* Summary
*/
return RTTestSummaryAndDestroy(hTest);
}
//
// 函数: InitInstance(HINSTANCE, int)
//
// 目的: 保存实例句柄并创建主窗口
//
// 注释:
//
// 在此函数中，我们在全局变量中保存实例句柄并
// 创建和显示主程序窗口。
//
BOOL InitInstance(HINSTANCE hInstance, int nCmdShow)
{
//test();
testInit();
HWND hWnd;
hInst = hInstance; // 将实例句柄存储在全局变量中
hWnd = CreateWindow(szWindowClass, szTitle, WS_OVERLAPPEDWINDOW,
CW_USEDEFAULT, 0, CW_USEDEFAULT, 0, NULL, NULL, hInstance, NULL);
if (!hWnd)
{
return FALSE;
}
ShowWindow(hWnd, nCmdShow);
UpdateWindow(hWnd);
return TRUE;
}
void
$module$Test (int testNumber)
{
TEST_STR *testStr;
/* Allocation de la structure */
if ((testStr = testInit(testNumber, "$module$",
$MODULE$_CLIENT_MBOX_REPLY_SIZE,
$MODULE$_ABORT_RQST,
$nbRequest$,
$module$TestRequestNameTab,
$module$TestRqstFuncTab,
$nbPosterData$,
$module$TestPosterNameTab,
$module$TestPosterShowFuncTab)) == NULL)
return;
/* Init specifiques */
$module$TestInitTask (testStr);
/* Fonction principale */
testMain(testStr);
}
int main( int argc, char *argv[] )
{
time_t shouldend;
/* Initialize ALUT. */
testInit( &argc, argv );
init( ( const ALbyte * ) ( ( argc == 1 ) ? WAVEFILE : argv[1] ) );
alSourcePlay( movingSource );
while( 1 ) {
shouldend = time( NULL );
if( ( shouldend - start ) > 40 ) {
break;
}
iterate( );
}
testExit( );
return EXIT_SUCCESS;
}
void cryptoRunTests(){
testInit();
testDesEcb();
testDesCfb();
testDesOfb();
testDesCbc();
testAes128Ecb();
testAes128Cfb();
testAes128Ofb();
testAes128Cbc();
testAes192Ecb();
testAes192Cfb();
testAes192Ofb();
testAes192Cbc();
testAes256Ecb();
testAes256Cfb();
testAes256Ofb();
testAes256Cbc();
}
void main_stack() {
//void main() {
//base type
// char s[10] = {'a', 'b', 'c', 'd', 'e','f', 'g', 'h', 'i', 'j'};
//
// ZStack* stack = zstackInit(10, typeByte);
//
// for (int i = 0; i < 12; i++) {
// zstackPushByte(stack, s[i]);
// }
//
// for (int i = 0; i < 12; i++) {
// printf("%c \n", zstackPopByte(stack));
// }
//ptr type
char* key[10] = {"aa", "bb", "cc", "dd", "ee", "ff", "gg", "hh", "ii", "jj"};
char* val[10] = {"aav", "bbv", "ccv", "ddv", "eev", "ffv", "ggv", "hhv", "iiv", "jjv"};
ZStack* stack2 = zstackInit(10, typePtr);
for (int i = 0; i < 12; i++) {
testNode* t1 = testInit(key[i], val[i]);
zstackPush(stack2, t1);
}
for (int i = 0; i < 12; i++) {
testNode* t = (testNode*)zstackPop(stack2);
if (t == NULL) {
break;
}
printf("key = %s, val = %s \n", t->key, t->val);
}
}
void testsReception(void) {
testInit();
testRun();
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
RCC_GetClocksFreq(&clocks);
USB_Interrupts_Config();
Set_USBClock();
USB_Init();
// Wait until device configured
//while(bDeviceState != CONFIGURED);
testInit();
LED0_ON;
systemReady = true;
//nrf_tx_mode_no_aa(addr,5,40);
nrf_rx_mode_dual(addr,5,40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
}
while (1)
{
uint8_t buf[64];
static uint8_t last_tx_done = 0;
if(ring_buf_pop(nrf_rx_buffer,buf,32)){
// get data from the adapter
switch(buf[0]){
case SET_ATT:
break;
case SET_MOTOR:
break;
case SET_MODE:
report_mode = buf[1];
break;
}
last_tx_done = 1;
}
if(tx_done){
tx_done = 0;
// report ACK success
last_tx_done = 1;
}
if(ring_buf_pop(nrf_tx_buffer,buf,32)){
if(last_tx_done){
last_tx_done = 0;
nrf_ack_packet(0, buf, 32);
}
}
if (frame_50Hz)
{
int16_t motor_val[4];
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
//memcpy(buf, accelSummedSamples100Hz, 12);
//memcpy(buf+12, gyroSummedSamples100Hz, 12);
//memcpy(buf+24, magSumed, 6);
if(report_mode == DT_ATT){
buf[0] = DT_ATT;
memcpy(buf + 1, &sensors.attitude200Hz[0], 12);
memcpy(buf + 13, &executionTime200Hz, 4);
motor_val[0] = motor[0];
motor_val[1] = motor[1];
motor_val[2] = motor[2];
motor_val[3] = motor[3];
memcpy(buf + 17, motor_val, 8);
usb_send_data(buf , 64);
executionTime50Hz = micros() - currentTime;
}else if(report_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
memcpy(buf + 1, gyroSummedSamples100Hz, 12);
memcpy(buf + 13, accelSummedSamples100Hz, 12);
memcpy(buf + 25, magSumed, 6);
}
//nrf_tx_packet(buf,16);
//if(nrf_rx_packet(buf,16) == NRF_RX_OK)
//{
// LED0_TOGGLE;
//}
ring_buf_push(nrf_tx_buffer, buf, 32);
}
if(frame_10Hz)
{
frame_10Hz = false;
magSumed[XAXIS] = magSum[XAXIS];
magSumed[YAXIS] = magSum[YAXIS];
magSumed[ZAXIS] = magSum[ZAXIS];
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
}
if (frame_100Hz)
{
frame_100Hz = false;
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
}
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
}
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
LED0_TOGGLE;
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
static void testManager(const char *port,int addr,FILE *file)
{
cmdInfo *pacmdInfo[2],*pcmdInfo;
threadInfo *pathreadInfo[2],*pthreadInfo;
asynUser *pasynUser;
asynStatus status;
int isConnected = 0;
int isEnabled = 0;
int yesNo;
int i;
pasynUser = pasynManager->createAsynUser(workCallback,0);
status = pasynManager->connectDevice(pasynUser,port,addr);
if(status!=asynSuccess) {
printf("connectDevice failed %s\n",pasynUser->errorMessage);
return;
}
if((testInit(port,addr,&pacmdInfo[0],&pathreadInfo[0],0,file))) return;
if((testInit(port,addr,&pacmdInfo[1],&pathreadInfo[1],1,file))) return;
/* if port is not connected try to connect*/
status = pasynManager->isConnected(pasynUser,&isConnected);
if(status!=asynSuccess) {
printf("isConnected failed %s\n",
pasynUser->errorMessage);
pasynManager->freeAsynUser(pasynUser);
return;
}
if(!isConnected) {
asynUser *pasynUserSave;
pcmdInfo = pacmdInfo[0];
pthreadInfo = pathreadInfo[0];
pcmdInfo->pthreadInfo = pthreadInfo;
pthreadInfo->pcmdInfo = pcmdInfo;
pcmdInfo->test = connect;
pasynUserSave = pcmdInfo->pasynUser;
pcmdInfo->pasynUser = pasynUser;
pasynUser->userPvt = pcmdInfo;
status = pasynManager->queueRequest(pasynUser,asynQueuePriorityConnect,0.0);
if(status!=asynSuccess) {
printf("port connect queueRequest failed\n");
return;
}
epicsEventMustWait(pcmdInfo->callbackDone);
status = pasynManager->isConnected(pasynUser,&isConnected);
if(status!=asynSuccess) {
printf("isConnected failed %s\n",
pasynUser->errorMessage);
return;
}
if(!isConnected) {
printf("not connected\n");
return;
}
status = pasynManager->isEnabled(pasynUser,&isEnabled);
if(status!=asynSuccess) {
printf("isEnabled failed %s\n",
pasynUser->errorMessage);
return;
}
if(!isEnabled) {
printf("not enabled\n");
return;
}
pcmdInfo->pasynUser = pasynUserSave;
}
status = pasynManager->freeAsynUser(pasynUser);
if(status) {
printf("freeAsynUser failed %s\n",pasynUser->errorMessage);
return;
}
pasynUser = pacmdInfo[0]->pasynUser;
pasynManager->canBlock(pasynUser,&yesNo);
if(!yesNo) {
fprintf(file,"\nport %s addr %d cant block so no block or cancel test\n",port,addr);
} else {
fprintf(file,"\nport %s addr %d\n",port,addr);
/* test lock */
fprintf(file,"test blockProcessCallback/unblockProcessCallback. thread0 first\n");
for(i=0; i<2; i++) {
pcmdInfo = pacmdInfo[i];
pthreadInfo = pathreadInfo[i];
pcmdInfo->test = testBlock;
strcpy(pcmdInfo->message,"blockProcessCallback/unblockProcessCallback");
epicsEventSignal(pthreadInfo->work);
epicsThreadSleep(.01);
}
for(i=0; i<2; i++) {
epicsEventMustWait(pathreadInfo[i]->done);
}
fprintf(file,"test unblockProcessCallback/unblockProcessCallback. thread1 first\n");
for(i=1; i>=0; i--) {
epicsEventSignal(pathreadInfo[i]->work);
epicsThreadSleep(.01);
}
for(i=0; i<2; i++) {
epicsEventMustWait(pathreadInfo[i]->done);
}
fprintf(file,"test cancelRequest\n");
for(i=0; i<1; i++) {
pacmdInfo[i]->test = testCancelRequest;
strcpy(pacmdInfo[i]->message,"cancelRequest");
epicsEventSignal(pathreadInfo[i]->work);
}
for(i=0; i<1; i++) {
epicsEventMustWait(pathreadInfo[i]->done);
}
}
/*now terminate and clean up*/
for(i=0; i<2; i++) {
pacmdInfo[i]->test = quit;
strcpy(pacmdInfo[i]->message,"quit");
epicsEventSignal(pathreadInfo[i]->work);
}
for(i=0; i<2; i++) {
epicsEventMustWait(pathreadInfo[i]->done);
}
for(i=0; i<2; i++) {
pthreadInfo = pathreadInfo[i];
pcmdInfo = pacmdInfo[i];
pasynUser = pacmdInfo[i]->pasynUser;
epicsEventDestroy(pthreadInfo->done);
epicsEventDestroy(pthreadInfo->work);
epicsEventDestroy(pcmdInfo->callbackDone);
status = pasynManager->freeAsynUser(pasynUser);
if(status) {
printf("freeAsynUser failed %s\n",pasynUser->errorMessage);
return;
}
pasynManager->memFree(pthreadInfo,pthreadInfo->size);
pasynManager->memFree(pcmdInfo,sizeof(cmdInfo));
}
}
int main( int argc, char *argv[] )
{
time_t shouldend;
start = time( NULL );
shouldend = time( NULL );
testInit( &argc, argv );
getExtensionEntries( );
init( ( const ALbyte * ) ( ( argc == 1 ) ? WAVEFILE : argv[1] ) );
palBombOnError( );
#if 0
alSourceStop( multis[0] );
fprintf( stderr, "Play a source\n" );
alSourcePlay( multis[0] );
fprintf( stderr, "Stop previous source\n" );
alSourceStop( multis[0] );
fprintf( stderr, "Stop a stopped source\n" );
alSourceStop( multis[0] );
fprintf( stderr, "Play a stopped source\n" );
alSourcePlay( multis[0] );
fprintf( stderr, "Play a playing source\n" );
alSourcePlay( multis[0] );
fprintf( stderr, "Delete a playing Source\n" );
alDeleteSources( 1, multis );
fprintf( stderr, "Play a deleted source\n" );
alSourcePlay( multis[0] );
#endif
alGetError( );
fprintf( stderr, "play\n" );
alSourcePlay( multis[0] );
sleep( 2 );
fprintf( stderr, "delete\n" );
alDeleteBuffers( 1, &boom );
fprintf( stderr, "Checking deferred deletion (shouldn't be valid)\n" );
if( alIsBuffer( boom ) ) {
fprintf( stderr, "deleted buffer is valid buffer\n" );
} else {
fprintf( stderr, "deleted buffer is not valid buffer\n" );
}
sleep( 2 );
fprintf( stderr, "stop\n" );
alSourceStop( multis[0] );
sleep( 2 );
fprintf( stderr, "Checking after source stop (shouldn't be valid)\n" );
if( alIsBuffer( boom ) ) {
fprintf( stderr, "deleted buffer is valid buffer\n" );
} else {
fprintf( stderr, "deleted buffer is not valid buffer\n" );
}
fprintf( stderr, "play again: prepare to hose\n" );
alSourcePlay( multis[0] );
fprintf( stderr, "error: %s\n", alGetString( alGetError( ) ) );
sleep( 2 );
testExit();
return EXIT_SUCCESS;
}