/***********************************************************************//** * @brief Test matrix functions * * Tests matrix functions. ***************************************************************************/ void TestGSparseMatrix::matrix_functions(void) { // Minimum double min = m_test.min(); // Check mimimum double value = 0.0; test_value(min, value, 0.0, "Test minimum function"); // Maximum double max = m_test.max(); // Check maximum value = g_matrix[0]; for (int i = 1; i < g_elements; ++i) { if (g_matrix[i] > value) { value = g_matrix[i]; } } test_value(max, value, 0.0, "Test maximum function"); // Sum double sum = m_test.sum(); // Check sum value = 0.0; for (int i = 0; i < g_elements; ++i) { value += g_matrix[i]; } test_value(sum, value, 1.0e-20, "Test sum function"); // Transpose function GSparseMatrix test1 = transpose(m_test); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix_trans(test1, 1.0, 0.0), "Test transpose(GSparseMatrix) function", "Unexpected transposed matrix:\n"+test1.print()); // Transpose method test1 = m_test; test1.transpose(); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix_trans(test1, 1.0, 0.0), "Test GSparseMatrix.transpose() method", "Unexpected transposed matrix:\n"+test1.print()); // Convert to general matrix GSparseMatrix test2 = GSparseMatrix(m_test); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test2, 1.0, 0.0), "Test GSparseMatrix(GSparseMatrix) constructor", "Unexpected GSparseMatrix:\n"+test2.print()); // Return return; }
/***********************************************************************//** * @brief Test matrix copy ***************************************************************************/ void TestGMatrixSparse::copy_matrix(void) { // Copy matrix GMatrixSparse test = m_test; // Test if original and compied matrices are correct test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test), "Test matrix copy operator", "Found:\n"+test.print()+"\nExpected:\n"+m_test.print()); // Return return; }
void __fastcall__ joker_hit(char pl){ char i,j; stop = 1; kill_joker(); matrix[joker_y][joker_x] = joker_tmp; sc = level*10; for (i=0;i<8;++i){ for(j=0;j<8;++j){ if (matrix[i][j] == joker_tmp){ print3x3(matrix[i][j]+8,j,i); } } } delay(20); for (i=0;i<8;++i){ for(j=0;j<8;++j){ if (matrix[i][j] == joker_tmp){ if (hits[joker_tmp]){ --hits[joker_tmp]; print_hits(); } print3x3(matrix[i][j]=EMPTY_SYMB,j,i); plot_score(sc,XOFFS+3*j,3*i+1); print_num(score[pl]+=sc,6,28,22-(players<<1)+(pl<<1)); } } } move_matrix(); check_matrix(1); stop = 0; }
// run a full DCM update round void AP_AHRS_DCM::update(void) { float delta_t; // tell the IMU to grab some data _imu->update(); // ask the IMU how much time this sensor reading represents delta_t = _imu->get_delta_time(); // Get current values for gyros _gyro_vector = _imu->get_gyro(); _accel_vector = _imu->get_accel(); // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); }
// run a full DCM update round void AP_AHRS_DCM::update(void) { float delta_t; // tell the IMU to grab some data _ins.update(); // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { _ra_sum.zero(); _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); }
int tst3(int n) {int ok; PM_matrix *a, *b, *val; printf("Test #3\n"); a = test_matrix(n); b = PM_create(a->nrow, a->ncol); PM_copy(b, a); /* find eigenvalues and eigenvectors */ val = PM_eigensys(a); printf("Eigenvalues for A:\n"); PM_print(val); printf("Eigenvectors for A:\n"); PM_print(a); /* verify */ ok = check_matrix(a, b, val); PM_destroy(val); PM_destroy(a); PM_destroy(b); return(ok);}
int main(void) { int *start; unsigned long nsegs, seg_size, stride_size; int i, ret; struct dma_addr addr; addr.addr = 0; addr.port = PORT; dma_bind_addr(&addr); for (i = 0; i < N * N; i++) mat_a[i] = i; start = mat_a + ROW * N + COL; nsegs = M; seg_size = M * sizeof(int); stride_size = (N - M) * sizeof(int); dma_gather_put(&addr, mat_b, start, seg_size, stride_size, nsegs); dma_fence(); ret = dma_send_error(); if (ret) return 0x10 | ret; if (check_matrix()) return 0x20; for (i = 0; i < M * M; i++) mat_b[i] *= 2; dma_scatter_get(&addr, start, mat_b, seg_size, stride_size, nsegs); dma_fence(); ret = dma_send_error(); if (ret) return 0x30 | ret; if (check_matrix()) return 0x40; return 0; }
// run a full DCM update round void AP_AHRS_DCM::update(bool skip_ins_update) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); float delta_t; if (_last_startup_ms == 0) { _last_startup_ms = AP_HAL::millis(); load_watchdog_home(); } if (!skip_ins_update) { // tell the IMU to grab some data AP::ins().update(); } const AP_InertialSensor &_ins = AP::ins(); // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // update trig values including _cos_roll, cos_pitch update_trig(); // update AOA and SSA update_AOA_SSA(); backup_attitude(); }
void __fastcall__ swap(char x, char y,char xswap,char yswap){ char i,j,c1,c2,x1,y1; x1 = x + xswap; y1 = y + yswap; if (matrix[y1][x1] == JOKER_SYMB) kill_joker(); clone(y,x,0); clone(y1,x1,1); c1 = matrix[y1][x1]; c2 = matrix[y][x]; print3x3(EMPTY_SYMB,x,y); print3x3(EMPTY_SYMB,x1,y1); for (i=0;i<24;++i){ vic->spr1_y -= yswap; vic->spr0_y += yswap; vic->spr1_x -= xswap; vic->spr0_x += xswap; for (j=0;j<SWAP_DELAY;++j); } matrix[y][x] = c1; matrix[y1][x1] = c2; print3x3(c1,x,y); print3x3(c2,x1,y1); chk_flg = 0; vic->spr_ena &= 252; check_matrix(1); if (chk_flg == 0){ //no success in swapping, then swap back vic->spr_ena |= 3; print3x3(EMPTY_SYMB,x,y); print3x3(EMPTY_SYMB,x1,y1); for (i=0;i<24;++i){ vic->spr1_y += yswap; vic->spr0_y -= yswap; vic->spr1_x += xswap; vic->spr0_x -= xswap; for (j=0;j<SWAP_DELAY;++j); } matrix[y][x] = c2; matrix[y1][x1] = c1; print3x3(c1,x1,y1); print3x3(c2,x,y); vic->spr_ena &= 252; } else{ //success in swapping, then check if moves are possible now. no_more_moves_flag = !(check_moves()); } }
size_t matrix_size(const dim_type dim[],ndim_type ndim) { check_matrix(dim,ndim); size_t size = 1; ndim_type i; for(i=0; i<ndim; i++) size *= dim[i]; return size; }
int main(int argc, char *argv[]) { std::random_device rd; generator_t gen(rd()); distribution_t dist(-10.0f, 10.0f); check_matrix(gen, dist); check_quaternion(gen, dist); return EXIT_SUCCESS; }
// run a full DCM update round void BC_AHRS::update(void) { float delta_t; // GYRO+ACCの取得 _ins.get_data(); // GYRO+ACCの取得にかかった時間を取得 delta_t = _ins.get_delta_time(); // 取得時間が0.2sec以上だったら捨てる // CopterにおいてArm時等にそうなる if (delta_t > 0.2f) { memset(&_ra_sum, 0, sizeof(_ra_sum)); // _ra_sumを0で埋めてる _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs // (超訳)ジャイロ値で方向余弦行列を更新 // ★チェックOK matrix_update(delta_t); // Normalize the DCM matrix // (超訳)方向余弦行列をノーマライズ // ★チェックOK normalize(); // Perform drift correction // (超訳)ドリフト補正を実施 drift_correction(delta_t); // paranoid check for bad values in the DCM matrix // (超訳)方向余弦行列中の不正値をチェック check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation // (超訳)ロール、ピッチ、ヨーを計算 euler_angles(); // update trig values including _cos_roll, cos_pitch // (超訳)必要な値(ex. rollのcos値,等)を計算 update_trig(); }
// run a full DCM update round void AP_AHRS_DCM::update(int16_t attitude[3], float delta_t) { // Get current values for gyros _gyro_vector = gyro_attitude; _accel_vector = accel; // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction //setCurrentProfiledActivity(DRIFT_CORRECTION); drift_correction(delta_t); // paranoid check for bad values in the DCM matrix //setCurrentProfiledActivity(CHECK_MATRIX); check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation //setCurrentProfiledActivity(EULER_ANGLES); euler_angles(); //setCurrentProfiledActivity(ANGLESOUTPUT); attitude[0] = roll * INT16DEG_PI_FACTOR; attitude[1] = pitch* INT16DEG_PI_FACTOR; attitude[2] = yaw * INT16DEG_PI_FACTOR; // Just for info: int16_t sensor = degrees(roll) * 10; debugOut.analog[0] = sensor; sensor = degrees(pitch) * 10; debugOut.analog[1] = sensor; sensor = degrees(yaw) * 10; if (sensor < 0) sensor += 3600; debugOut.analog[2] = sensor; }
int tst2(int n) {int rv; PM_matrix *a, *b; printf("Test #2\n"); a = test_matrix(n); b = PM_create(a->nrow, a->ncol); PM_copy(b, a); /* find the right eigenvectors */ rv = PM_eigenvectors(a); if (rv == TRUE) {printf("Eigenvectors for A:\n"); PM_print(a);}; /* verify */ rv &= check_matrix(a, b, NULL); PM_destroy(a); return(rv);}
int main(int argc, char**argv) { uint32_t r, c, o; char name[64]; name[63]=0; for(r=1; r<16; r++) { for(c=1; c<16; c++) { snprintf(name, 63, "zero[%d, %d]", r, c); check_matrix(name, AMBIX_MATRIX_ZERO, r, c); snprintf(name, 63, "one[%d, %d]", r, c); check_matrix(name, AMBIX_MATRIX_ONE, r, c); snprintf(name, 63, "identity[%d, %d]", r, c); check_matrix(name, AMBIX_MATRIX_IDENTITY, r, c); } } check_matrix("FuMa[ 1, 1]", AMBIX_MATRIX_FUMA, 1, 1); check_matrix("FuMa[ 4, 3]", AMBIX_MATRIX_FUMA, 4, 3); check_matrix("FuMa[ 4, 4]", AMBIX_MATRIX_FUMA, 4, 4); check_matrix("FuMa[ 9, 5]", AMBIX_MATRIX_FUMA, 9, 5); check_matrix("FuMa[ 9, 6]", AMBIX_MATRIX_FUMA, 9, 6); check_matrix("FuMa[ 9, 9]", AMBIX_MATRIX_FUMA, 9, 9); check_matrix("FuMa[16, 7]", AMBIX_MATRIX_FUMA, 16, 7); check_matrix("FuMa[16, 8]", AMBIX_MATRIX_FUMA, 16, 8); check_matrix("FuMa[16,11]", AMBIX_MATRIX_FUMA, 16, 11); check_matrix("FuMa[16,16]", AMBIX_MATRIX_FUMA, 16, 16); check_inversion("FuMa[ 1, 1]", AMBIX_MATRIX_FUMA, 1, 1); check_inversion("FuMa[ 4, 3]", AMBIX_MATRIX_FUMA, 4, 3); check_inversion("FuMa[ 4, 4]", AMBIX_MATRIX_FUMA, 4, 4); check_inversion("FuMa[ 9, 5]", AMBIX_MATRIX_FUMA, 9, 5); check_inversion("FuMa[ 9, 6]", AMBIX_MATRIX_FUMA, 9, 6); check_inversion("FuMa[ 9, 9]", AMBIX_MATRIX_FUMA, 9, 9); check_inversion("FuMa[16, 7]", AMBIX_MATRIX_FUMA, 16, 7); check_inversion("FuMa[16, 8]", AMBIX_MATRIX_FUMA, 16, 8); check_inversion("FuMa[16,11]", AMBIX_MATRIX_FUMA, 16, 11); check_inversion("FuMa[16,16]", AMBIX_MATRIX_FUMA, 16, 16); for(o=1; o<6; o++) { uint32_t chan=ambix_order2channels(o); snprintf(name, 63, "n3d2snd3d[%d, %d]", chan, chan); check_matrix(name, AMBIX_MATRIX_N3D, chan, chan); snprintf(name, 63, "sn3d2n3d[%d, %d]", chan, chan); check_matrix(name, AMBIX_MATRIX_TO_N3D, chan, chan); snprintf(name, 63, "n3d[%d, %d]", chan, chan); check_inversion(name, AMBIX_MATRIX_N3D, 1, 1); snprintf(name, 63, "sid2acn[%d, %d]", chan, chan); check_matrix(name, AMBIX_MATRIX_SID, chan, chan); snprintf(name, 63, "acn2sid[%d, %d]", chan, chan); check_matrix(name, AMBIX_MATRIX_TO_SID, chan, chan); snprintf(name, 63, "sid[%d, %d]", chan, chan); check_inversion(name, AMBIX_MATRIX_SID, 1, 1); } pass(); return 0; }
int main(int argc, char *argv[]) { blocking_entry(); long long int start; long long int end; start = get_micro_clock(); int j, k, noproc, me_no; double sum; double t1, t2; pthread_t *threads; pthread_attr_t pthread_custom_attr; parm *arg; int n, i; if (argc != 3) { printf("Usage: %s n dim\n where n is no. of thread and dim is the size of matrix\n", argv[0]); exit(1); } n = atoi(argv[1]); if ((n < 1) || (n > MAX_THREAD)) { printf("The no of thread should between 1 and %d.\n", MAX_THREAD); exit(1); } NDIM = atoi(argv[2]); pthread_mutex_init(&lock, NULL); init_matrix(&a); init_matrix(&b); init_matrix(&c); for (i = 0; i < NDIM; i++) for (j = 0; j < NDIM; j++) { a[i][j] = i + j; b[i][j] = i + j; } threads = (pthread_t*) malloc(n * sizeof(pthread_t)); pthread_attr_init(&pthread_custom_attr); arg = (parm*) malloc(sizeof(parm) * n); /* setup barrier */ /* Start up thread */ /* Spawn thread */ for (i = 0; i < n; i++) { arg[i].id = i; arg[i].noproc = n; arg[i].dim = NDIM; arg[i].a = a; arg[i].b = b; arg[i].c = c; pthread_create(&threads[i], &pthread_custom_attr, worker, (void*) (arg+i)); } for (i = 0; i < n; i++) { pthread_join(threads[i], NULL); } /* print_matrix(NDIM); */ check_matrix(NDIM); free(arg); end = get_micro_clock(); fprintf(stderr, "> application runtime: %lld microseconds\n", end - start); return 0; }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // rotate accelerometer values into the earth frame for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (_ins.get_accel_health(i)) { /* by using get_delta_velocity() instead of get_accel() the accel value is sampled over the right time delta for each sensor, which prevents an aliasing effect */ Vector3f delta_velocity; float delta_velocity_dt; _ins.get_delta_velocity(i, delta_velocity); delta_velocity_dt = _ins.get_delta_velocity_dt(i); if (delta_velocity_dt > 0) { _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt); // integrate the accel vector in the earth frame between GPS readings _ra_sum[i] += _accel_ef[i] * deltat; } } } //update _accel_ef_blended if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) { float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f; // slew _imu1_weight over one second _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat); _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight); } else { _accel_ef_blended = _accel_ef[_ins.get_primary_accel()]; } // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps() || _gps.status() < AP_GPS::GPS_OK_FIX_3D || _gps.num_sats() < _gps_minsats) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. // // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2f) { // not enough time has accumulated return; } float airspeed; if (airspeed_sensor_enabled()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = AP_HAL::millis(); _have_gps_lock = false; } else { if (_gps.last_fix_time_ms() == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = _gps.velocity(); last_correction_time = _gps.last_fix_time_ms(); if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } if (have_gps()) { // use GPS for positioning with any fix, even a 2D fix _last_lat = _gps.location().lat; _last_lng = _gps.location().lng; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we can update using // dead-reckoning from then on _have_position = true; } else { // update dead-reckoning position estimate _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; GA_e = Vector3f(0, 0, -1.0f); if (_ra_deltat <= 0) { // waiting for more data return; } bool using_gps_corrections = false; float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS); if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) { float v_scale = gps_gain.get() * ra_scale; Vector3f vdelta = (velocity - _last_velocity) * v_scale; GA_e += vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information _last_failure_ms = AP_HAL::millis(); return; } using_gps_corrections = true; } // calculate the error term in earth frame. // we do this for each available accelerometer then pick the // accelerometer that leads to the smallest error term. This takes // advantage of the different sample rates on different // accelerometers to dramatically reduce the impact of aliasing // due to harmonics of vibrations that match closely the sampling // rate of our accelerometers. On the Pixhawk we have the LSM303D // running at 800Hz and the MPU6000 running at 1kHz, by combining // the two the effects of aliasing are greatly reduced. Vector3f error[INS_MAX_INSTANCES]; float error_dirn[INS_MAX_INSTANCES]; Vector3f GA_b[INS_MAX_INSTANCES]; int8_t besti = -1; float best_error = 0; for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (!_ins.get_accel_health(i)) { // only use healthy sensors continue; } _ra_sum[i] *= ra_scale; // get the delayed ra_sum to match the GPS lag if (using_gps_corrections) { GA_b[i] = ra_delayed(i, _ra_sum[i]); } else { GA_b[i] = _ra_sum[i]; } if (GA_b[i].is_zero()) { // wait for some non-zero acceleration information continue; } GA_b[i].normalize(); if (GA_b[i].is_inf()) { // wait for some non-zero acceleration information continue; } error[i] = GA_b[i] % GA_e; // Take dot product to catch case vectors are opposite sign and parallel error_dirn[i] = GA_b[i] * GA_e; float error_length = error[i].length(); if (besti == -1 || error_length < best_error) { besti = i; best_error = error_length; } // Catch case where orientation is 180 degrees out if (error_dirn[besti] < 0.0f) { best_error = 1.0f; } } if (besti == -1) { // no healthy accelerometers! _last_failure_ms = AP_HAL::millis(); return; } _active_accel_instance = besti; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = norm(GA_e.x, GA_e.y); // equation 11 float theta = atan2f(GA_b[besti].y, GA_b[besti].x); // equation 12 Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z); // step 6 error = GA_b[besti] % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (AP_AHRS_DCM::use_compass()) { if (have_gps() && is_equal(gps_gain.get(), 1.0f)) { error[besti].z *= sinf(fabsf(roll)); } else { error[besti].z = 0; } } // if ins is unhealthy then stop attitude drift correction and // hope the gyros are OK for a while. Just slowly reduce _omega_P // to prevent previous bad accels from throwing us off if (!_ins.healthy()) { error[besti].zero(); } else { // convert the error term to body frame error[besti] = _dcm_matrix.mul_transpose(error[besti]); } if (error[besti].is_nan() || error[besti].is_inf()) { // don't allow bad values check_matrix(); _last_failure_ms = AP_HAL::millis(); return; } _error_rp = 0.8f * _error_rp + 0.2f * best_error; // base the P gain on the spin rate float spin_rate = _omega.length(); // sanity check _kp value if (_kp < AP_AHRS_RP_P_MIN) { _kp = AP_AHRS_RP_P_MIN; } // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error[besti] * _P_gain(spin_rate) * _kp; if (use_fast_gains()) { _omega_P *= 8; } if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && _gps.ground_speed() < GPS_SPEED_MIN && _ins.get_accel().x >= 7 && pitch_sensor > -3000 && pitch_sensor < 3000) { // assume we are in a launch acceleration, and reduce the // rp gain by 50% to reduce the impact of GPS lag on // takeoff attitude when using a catapult _omega_P *= 0.5f; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error[besti] * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step memset(&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; }
/** * Main driver code for the parallel lab. Generates the matrix of the * specified size, initiates the decomposition and checking * routines. */ int main (int argc, char *argv[]) { int size = 0; double *a = NULL; double *lu = NULL; clock_t start, time1, time2; struct timeval start_timeval, end_timeval; double elapsed_secs, elapsed_total_secs, cpu_secs; /* Bail out if we don't have the correct number of parameters */ if (argc!=2) { printf("This program is used to decompose a (random) matrix A into its components L and U.\n"); printf("Usage: %s <matrix size>\n", argv[0]); return -1; } size = atoi(argv[1]); /* Adjust matrix size */ if (size < MIN_MATRIX_SIZE) { printf("Setting matrix size to minimum value %d.\n", MIN_MATRIX_SIZE); size = MIN_MATRIX_SIZE; } else if (size > MAX_MATRIX_SIZE) { printf("Setting matrix size to maximum value %d.\n", MAX_MATRIX_SIZE); size = MAX_MATRIX_SIZE; } /* Generate data. */ printf("LU matrix decomposition, starting warmup...\n"); printf(" - Generating a %i * %i matrix\n", size, size); a = (double*)malloc(sizeof(double)*size*size); lu = (double*)malloc(sizeof(double)*size*size); if (a==NULL || lu==NULL) { printf("Not enough memory!\n"); return -1; } fill_matrix(a, size); print_matrix(a, size); memcpy(lu, a, sizeof(double)*size*size); /* Start LU decomposition. */ printf("Decomposing the matrix into its components...\n"); gettimeofday(&start_timeval, NULL); start = clock(); decompose_matrix(lu, size); time1 = clock()-start; gettimeofday(&end_timeval, NULL); elapsed_total_secs = elapsed_secs = (double)timediff_ms(&start_timeval, &end_timeval)/1000.0; cpu_secs = (double)(time1)/CLOCKS_PER_SEC; /* Verify resulting decomposition. */ printf("Checking result...\n"); print_matrix(lu, size); gettimeofday(&start_timeval, NULL); start = clock(); if (check_matrix(lu, a, size)) printf("The computation seems correct\n"); else printf("The computation seems not correct\n"); time2 = clock()-start; gettimeofday(&end_timeval, NULL); /* Output stats. */ printf("\nDecomposition time: %.2fs CPU, %.2fs elapsed, %.1f%% speedup\n", cpu_secs, elapsed_secs, cpu_secs/elapsed_secs*100.0); elapsed_secs = (double)timediff_ms(&start_timeval, &end_timeval)/1000.0; elapsed_total_secs += elapsed_secs; cpu_secs = (double)(time2)/CLOCKS_PER_SEC; printf("Checking time: %.2fs CPU, %.2fs elapsed, %.1f%% speedup\n", cpu_secs, elapsed_secs, cpu_secs/elapsed_secs*100.0); cpu_secs = (double)(time1+time2)/CLOCKS_PER_SEC; printf("Overall time: %.2fs CPU, %.2fs elapsed, %.1f%% speedup\n", cpu_secs, elapsed_total_secs, cpu_secs/elapsed_total_secs*100.0); /* Free resources. */ free(lu); free(a); return 0; }
int main(int argc, char *argv[]) { int nrep = 1, BS = 0; //parseArgs(argc,argv); double cusp_time; int ok = -1; double init_time = omp_get_wtime(); //init_load(argc,argv, &_numRows, &_numCols, &_numValues, _numericalValues, _rowOffsets,_colIndexValues, _vector, _output_vector,&check_res,&just_analyse); if (argc < 2) { exit(EXIT_FAILURE); } char const *mmfile = argv[1]; printf("Matrix File %s\n", mmfile); int m, n, nz; int *i_idx, *j_idx; double *a; check_res = 1; just_analyse = 0; if (argc == 3) if (argv[2][0] == 'a') { printf("Analyzing matrix...\n"); just_analyse = 1; } else check_res = 0; //read_mm_matrix (mmfile, &m, &n, &nz, &i_idx, &j_idx, &a); //enum sparse_matrix_file_format_t informat; struct sparse_matrix_t *A = load_sparse_matrix(MATRIX_MARKET, argv[1]); if (valid_sparse_matrix(A)) printf("\n### Sparse matrix is valid ###\n\n"); else printf("\n### Invalid sparse matrix! ###\n\n"); struct csr_matrix_t *B = NULL; B = coo_to_csr(A->repr); init_time = omp_get_wtime() - init_time; _numericalValues = (double *) B->values; if (just_analyse) { check_matrix(B->m, B->n, B->nnz, _numericalValues, B->colidx, B->rowptr); return 0; } _vector = malloc(sizeof(double) * B->m); _output_vector = malloc(sizeof(double) * B->m); for (int i = 0; i < B->m; ++i) _vector[i] = i; double ss_time; _numRows = (uint) B->m; _numValues = (uint) B->nnz; _rowOffsets = (uint) B->rowptr; _colIndexValues = (uint) B->colidx; int tmp; double db_time = omp_get_wtime(); #pragma omp target device(acc) #pragma omp task inout([_numValues]_numericalValues, [_numValues]_colIndexValues, [_numRows]_rowOffsets, [_numRows]_vector , [_numRows]_output_vector ) #pragma omp teams num_teams(1) thread_limit(1) { } #pragma omp taskwait noflush db_time = omp_get_wtime() - db_time; ss_time = omp_get_wtime(); SpMV_CSR(_numRows, _numValues, _numericalValues, _colIndexValues, _rowOffsets, _vector, _output_vector); #pragma omp taskwait noflush ss_time = omp_get_wtime() - ss_time; #pragma omp taskwait FILE *fp; fp = fopen("result.txt", "w+"); for (int i = 0; i < B->m; ++i) fprintf(fp, "%.2lf\n", _output_vector[i]); fclose(fp); // if(check_res!=0) // { // ok = check_result(_output_vector,_vector); // cusp_time = get_cusp_time(); // } unsigned long nops = (unsigned long) 2 * (B->m + B->nnz); prtspeed(_numRows, BS, ss_time, init_time, db_time, 0, 0, ok, nops); return 0; }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Matrix3f temp_dcm = _dcm_matrix; Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // apply trim temp_dcm.rotate(_trim); // rotate accelerometer values into the earth frame _accel_ef = temp_dcm * _accel_vector; // integrate the accel vector in the earth frame between GPS readings _ra_sum += _accel_ef * deltat; // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps()) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. // // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2) { // not enough time has accumulated return; } float airspeed; if (_airspeed && _airspeed->use()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = hal.scheduler->millis(); _have_gps_lock = false; // update position delta for get_position() _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } else { if (_gps->last_fix_time == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down()); last_correction_time = _gps->last_fix_time; if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // remember position for get_position() _last_lat = _gps->latitude; _last_lng = _gps->longitude; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we update using // dead-reckoning from then on _have_position = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } /* * The barometer for vertical velocity is only enabled if we got * at least 5 pressure samples for the reading. This ensures we * don't use very noisy climb rate data */ if (_baro_use && _barometer != NULL && _barometer->get_pressure_samples() >= 5) { // Z velocity is down velocity.z = -_barometer->get_climb_rate(); } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS); Vector3f vdelta = (velocity - _last_velocity) * v_scale; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. vdelta.z = constrain(vdelta.z, -0.5, 0.5); GA_e = Vector3f(0, 0, -1.0) + vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information return; } // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS); float length = GA_b.length(); if (length > 1.0) { GA_b /= length; if (GA_b.is_inf()) { // wait for some non-zero acceleration information return; } } Vector3f error = GA_b % GA_e; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = pythagorous2(GA_e.x, GA_e.y); // equation 11 float theta = atan2(GA_b.y, GA_b.x); // equation 12 Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z); // step 6 error = GA_b % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (_compass && _compass->use_for_yaw()) { if (have_gps() && gps_gain == 1.0) { error.z *= sin(fabs(roll)); } else { error.z = 0; } } // convert the error term to body frame error = _dcm_matrix.mul_transpose(error); if (error.is_nan() || error.is_inf()) { // don't allow bad values check_matrix(); return; } _error_rp_sum += error.length(); _error_rp_count++; // base the P gain on the spin rate float spin_rate = _omega.length(); // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error * _P_gain(spin_rate) * _kp; if (_fast_ground_gains) { _omega_P *= 8; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step _ra_sum.zero(); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; if (_have_gps_lock && _fly_forward) { // update wind estimate estimate_wind(velocity); } }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // rotate accelerometer values into the earth frame _accel_ef = _dcm_matrix * _ins.get_accel(); // integrate the accel vector in the earth frame between GPS readings _ra_sum += _accel_ef * deltat; // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps() || _gps->status() < GPS::GPS_OK_FIX_3D || _gps->num_sats < _gps_minsats) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. // // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2f) { // not enough time has accumulated return; } float airspeed; if (_airspeed && _airspeed->use()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = hal.scheduler->millis(); _have_gps_lock = false; } else { if (_gps->last_fix_time == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down()); last_correction_time = _gps->last_fix_time; if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } if (have_gps()) { // use GPS for positioning with any fix, even a 2D fix _last_lat = _gps->latitude; _last_lng = _gps->longitude; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we can update using // dead-reckoning from then on _have_position = true; } else { // update dead-reckoning position estimate _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; GA_e = Vector3f(0, 0, -1.0f); bool using_gps_corrections = false; if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) { float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS); Vector3f vdelta = (velocity - _last_velocity) * v_scale; GA_e += vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information return; } using_gps_corrections = true; } // calculate the error term in earth frame. _ra_sum /= (_ra_deltat * GRAVITY_MSS); // get the delayed ra_sum to match the GPS lag Vector3f GA_b; if (using_gps_corrections) { GA_b = ra_delayed(_ra_sum); } else { GA_b = _ra_sum; } GA_b.normalize(); if (GA_b.is_inf()) { // wait for some non-zero acceleration information return; } Vector3f error = GA_b % GA_e; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = pythagorous2(GA_e.x, GA_e.y); // equation 11 float theta = atan2f(GA_b.y, GA_b.x); // equation 12 Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z); // step 6 error = GA_b % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (use_compass()) { if (have_gps() && gps_gain == 1.0f) { error.z *= sinf(fabsf(roll)); } else { error.z = 0; } } // if ins is unhealthy then stop attitude drift correction and // hope the gyros are OK for a while. Just slowly reduce _omega_P // to prevent previous bad accels from throwing us off if (!_ins.healthy()) { error.zero(); } else { // convert the error term to body frame error = _dcm_matrix.mul_transpose(error); } if (error.is_nan() || error.is_inf()) { // don't allow bad values check_matrix(); return; } _error_rp_sum += error.length(); _error_rp_count++; // base the P gain on the spin rate float spin_rate = _omega.length(); // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error * _P_gain(spin_rate) * _kp; if (_flags.fast_ground_gains) { _omega_P *= 8; } if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D && _gps->ground_speed_cm < GPS_SPEED_MIN && _ins.get_accel().x >= 7 && pitch_sensor > -3000 && pitch_sensor < 3000) { // assume we are in a launch acceleration, and reduce the // rp gain by 50% to reduce the impact of GPS lag on // takeoff attitude when using a catapult _omega_P *= 0.5f; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step _ra_sum.zero(); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; }
int main (int argc, char *argv[]){ // total start time long int exec_time_nsec=0; struct timespec time1, time2; clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1); int j, k, noproc, me_no; double sum; double t1, t2; time_t startwtime, endwtime; pthread_t *threads; pthread_attr_t pthread_custom_attr; parm *arg; int n, i; startwtime = time (NULL); for (i = 0; i < NDIM; i++){ for (j = 0; j < NDIM; j++) { a[i][j] = i + j; b[i][j] = i + j; } } if (argc != 2) { printf ("Usage: %s n\n where n is no. of thread\n", argv[0]); exit (1); } n = atoi (argv[1]); if ((n < 1) || (n > MAX_THREAD)) { printf ("The no of thread should between 1 and %d.\n", MAX_THREAD); exit (1); } threads = (pthread_t *) malloc (n * sizeof (pthread_t)); pthread_attr_init (&pthread_custom_attr); arg = (parm *) malloc (sizeof (parm) * n); /* setup barrier */ /* Start up thread */ /* Spawn thread */ for (i = 0; i < n; i++) { arg[i].id = i; arg[i].noproc = n; arg[i].dim = NDIM; arg[i].a = &a; arg[i].b = &b; arg[i].c = &c; pthread_create (&threads[i], &pthread_custom_attr, worker, (void *) (arg + i)); } for (i = 0; i < n; i++) { pthread_join (threads[i], NULL); } /* print_matrix(NDIM); */ check_matrix (NDIM); free (arg); endwtime = time (NULL); printf ("wall clock time = %ld\n", (endwtime - startwtime)); clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2); exec_time_nsec+=time2.tv_nsec-time1.tv_nsec; printf("Total program run in %f sec(%ld nsec) ...\n",(double)exec_time_nsec/1000000000,exec_time_nsec); return 0; }
void __fastcall__ check_matrix(unsigned char fo){ unsigned char i,j,x1,x2,n,s; unsigned char ck = 0; stop = 1; memset(backup,EMPTY_SYMB,64); s = 0; for (i=0;i<8;++i){ for (x1=0;x1<6;++x1){ x2 = x1 + 1; while (matrix[i][x1] == matrix[i][x2] && x2 < 8) ++x2; if (x2 - x1 > 2){ for (j=x1;j<x2;++j){ backup[i][j] = matrix[i][j]; } n = x2 - x1 - 3; if ((sc = 10*level << n << (fo-1)) >= 999) sc = 999; print_num(score[pl]+=sc,6,28,22 - (players<<1) + (pl<<1)); scores[s].num = sc; scores[s].x = XOFFS+x1*3 + x_offsets[n]; scores[s].y = i*3+1; ++s; } if ((x1 = x2-1) >= 6) break; } } for (j=0;j<8;++j){ for (x1=0;x1<6;++x1){ x2 = x1 + 1; while (matrix[x1][j] == matrix[x2][j] && x2 < 8) ++x2; if (x2 - x1 > 2){ for (i=x1;i<x2;++i){ backup[i][j] = matrix[i][j]; } n = x2 -x1 - 3; if ((sc = 10*level << n << (fo-1)) >= 999) sc= 999; print_num(score[pl]+=sc,6,28,22 - (players<<1) + (pl<<1)); scores[s].num = sc; scores[s].x = XOFFS + j*3; scores[s].y = x1*3 + y_offsets[n]; ++s; } if ((x1 = x2-1) >= 6) break; } } for (i=0;i<8;++i){ for (j=0;j<8;++j){ if (backup[i][j] != EMPTY_SYMB){ ck = chk_flg = 1; print3x3(backup[i][j]+8,j,i); } } } if (ck) delay(40); for (i=0;i<8;++i){ for(j=0;j<8;++j){ if (backup[i][j] != EMPTY_SYMB){ print3x3(EMPTY_SYMB,j,i); matrix[i][j] = EMPTY_SYMB; if ((time1 += time_bonus[level-1]*fo*TIME_BONUS) > 319) time1 = 319; if (hits[backup[i][j]]) --hits[backup[i][j]]; display_time(); } } } for (n=0;n<s;++n){ plot_score(scores[n].num,scores[n].x,scores[n].y); } if (team != 0 && fo == 1){ tt ^=1; *(char*)0xd02e ^= 3; } if (ck){ print_hits(); delay(18); move_matrix(); check_matrix(++fo); } stop = 0; }
/***********************************************************************//** * @brief Test matrix functions * * Tests matrix functions. ***************************************************************************/ void TestGSymMatrix::matrix_functions(void) { // Minimum double min = m_test.min(); // Check mimimum double value = g_matrix[0]; for (int row = 0; row < g_rows; ++row) { for (int col = 0; col < g_cols; ++col) { if (g_matrix[col+row*g_cols] < value) { value = g_matrix[col+row*g_cols]; } } } test_value(min, value, 0.0, "Test minimum function"); // Maximum double max = m_test.max(); // Check maximum value = g_matrix[0]; for (int row = 0; row < g_rows; ++row) { for (int col = 0; col < g_cols; ++col) { if (g_matrix[col+row*g_cols] > value) { value = g_matrix[col+row*g_cols]; } } } test_value(max, value, 0.0, "Test maximum function"); // Sum double sum = m_test.sum(); // Check sum value = 0.0; for (int row = 0; row < g_rows; ++row) { for (int col = 0; col < g_cols; ++col) { value += g_matrix[col+row*g_cols]; } } test_value(sum, value, 1.0e-20, "Test sum function"); // Transpose function GSymMatrix test1 = transpose(m_test); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test1, 1.0, 0.0), "Test transpose(GSymMatrix) function", "Unexpected transposed matrix:\n"+test1.print()); // Transpose method test1 = m_test; test1.transpose(); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test1, 1.0, 0.0), "Test GSymMatrix.transpose() method", "Unexpected transposed matrix:\n"+test1.print()); // Convert to general matrix GMatrix test2 = GMatrix(m_test); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test2, 1.0, 0.0), "Test GMatrix(GSymMatrix) constructor", "Unexpected GMatrix:\n"+test2.print()); // Extract lower triangle test2 = m_test.extract_lower_triangle(); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix_lt(test2, 1.0, 0.0), "Test GSymMatrix.extract_lower_triangle() method", "Unexpected GMatrix:\n"+test2.print()); // Extract upper triangle test2 = m_test.extract_upper_triangle(); test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix_ut(test2, 1.0, 0.0), "Test GSymMatrix.extract_upper_triangle() method", "Unexpected GMatrix:\n"+test2.print()); // Return return; }
/***********************************************************************//** * @brief Test matrix arithmetics * * Tests matrix arithmetics. ***************************************************************************/ void TestGSymMatrix::matrix_arithmetics(void) { // -GSymMatrix GSymMatrix test = -m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, -1.0, 0.0), "Test -GSymMatrix", test.print()); // GSymMatrix += GSymMatrix test = m_test; test += m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 2.0, 0.0), "Test GSymMatrix += GSymMatrix", test.print()); // GSymMatrix -= GSymMatrix test = m_test; test -= m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 0.0, 0.0), "Test GSymMatrix -= GSymMatrix", test.print()); // GSymMatrix *= 3.0 test = m_test; test *= 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 3.0, 0.0), "Test GSymMatrix *= 3.0", test.print()); // GSymMatrix /= 3.0 test = m_test; test /= 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 1.0/3.0, 0.0), "Test GSymMatrix /= 3.0", test.print()); // GSymMatrix + GSymMatrix test = m_test + m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 2.0, 0.0), "Test GSymMatrix + GSymMatrix", test.print()); // GSymMatrix - GSymMatrix test = m_test - m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 0.0, 0.0), "Test GSymMatrix - GSymMatrix", test.print()); // GSymMatrix * 3.0 test = m_test * 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 3.0, 0.0), "Test GSymMatrix * 3.0", test.print()); // 3.0 * GSymMatrix test = 3.0 * m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 3.0, 0.0), "Test 3.0 * GSymMatrix", test.print()); // GSymMatrix / 3.0 test = m_test / 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 1.0/3.0, 0.0), "Test GSymMatrix / 3.0", test.print()); // Test invalid matrix addition test_try("Test invalid matrix addition"); try { test = m_test; test += m_bigger; test_try_failure("Expected GException::matrix_mismatch exception."); } catch (GException::matrix_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Return return; }
/***********************************************************************//** * @brief Test matrix operations * * Tests matrix*vector and matrix*matrix multiplication operations. ***************************************************************************/ void TestGSymMatrix::matrix_operations(void) { // Perform vector multiplication GVector test1 = m_test * v_test; // Check if the result vector is as expected GVector ref1 = test1; bool result = true; for (int row = 0; row < g_rows; ++row) { double value = 0.0; for (int col = 0; col < g_cols; ++col) { value += g_matrix[col+row*g_cols] * g_vector[col]; } ref1[row] = value; if (test1[row] != value) { result = false; break; } } // Test if original matrix and result vector are correct test_assert(check_matrix(m_test), "Test source matrix"); test_assert(result, "Test matrix*vector multiplication", "Found:\n"+test1.print()+"\nExpected:\n"+ref1.print()); // Test incompatible vector multiplication test_try("Test incompatible matrix*vector multiplication"); try { GVector test2 = m_bigger * v_test; test_try_failure(); } catch (GException::matrix_vector_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Test matrix multiplication GSymMatrix test3 = m_test * m_test; // Check if the result matrix is as expected GSymMatrix ref3 = test3; result = true; for (int row = 0; row < test3.rows(); ++row) { for (int col = 0; col < test3.cols(); ++col) { double value = 0.0; for (int i = 0; i < g_cols; ++i) { value += g_matrix[i+row*g_cols] * g_matrix[i+col*g_cols]; } ref3(row,col) = value; if (test3(row,col) != value) { result = false; break; } } } // Test if original matrix and result matrix are correct test_assert(check_matrix(m_test), "Test source matrix"); test_assert(result, "Test matrix multiplication", "Found:\n"+test3.print()+"\nExpected:\n"+ref3.print()); test_assert(test3.rows() == g_rows, "Test number of rows of result matrix"); test_assert(test3.cols() == g_cols, "Test number of columns of result matrix"); // Test incompatible matrix multiplication test_try("Test incompatible matrix multiplication"); try { GSymMatrix test4 = m_test * m_bigger; test_try_failure("Expected GException::matrix_mismatch exception."); } catch (GException::matrix_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Test another incompatible matrix multiplication test_try("Test incompatible matrix multiplication"); try { GSymMatrix test5 = m_bigger * m_test; test_try_failure("Expected GException::matrix_mismatch exception."); } catch (GException::matrix_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Return return; }
result_type result() const { return !interrupt && check_matrix(m_mask, base_t::matrix()); }
/***********************************************************************//** * @brief Test matrix operations * * Tests matrix*vector and matrix*matrix multiplication operations. ***************************************************************************/ void TestGMatrixSparse::matrix_operations(void) { // Perform vector multiplication GVector test1 = m_test * v_test; // Check result GVector ref1(g_rows); for (int i = 0; i < g_elements; ++i) { ref1[g_row[i]] += g_matrix[i] * v_test[g_col[i]]; } bool result = true; for (int i = 0; i < g_rows; ++i) { if (ref1[i] != test1[i]) { result = false; break; } } // Test if original matrix and result vector are correct test_assert(check_matrix(m_test), "Test source matrix"); test_assert(result, "Test matrix*vector multiplication", "Found:\n"+test1.print()+"\nExpected:\n"+ref1.print()); // Test incompatible vector multiplication test_try("Test incompatible matrix*vector multiplication"); try { GVector test2 = m_bigger * v_test; test_try_failure("Expected GException::matrix_vector_mismatch exception."); } catch (GException::matrix_vector_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Test matrix multiplication GMatrixSparse test3 = m_test * m_test.transpose(); // Check if the result matrix is as expected GMatrixSparse ref3(g_rows, g_rows); for (int row = 0; row < g_rows; ++row) { for (int col = 0; col < g_rows; ++col) { double value = 0.0; for (int i = 0; i < g_cols; ++i) { double ref_value_1 = 0.0; double ref_value_2 = 0.0; for (int k = 0; k < g_elements; ++k) { if (g_row[k] == row && g_col[k] == i) { ref_value_1 = g_matrix[k]; break; } } for (int k = 0; k < g_elements; ++k) { if (g_row[k] == col && g_col[k] == i) { ref_value_2 = g_matrix[k]; break; } } value += ref_value_1 * ref_value_2; } ref3(row,col) = value; if (test3(row,col) != value) { result = false; break; } } } // Test if original matrix and result matrix are correct test_assert(check_matrix(m_test), "Test source matrix"); test_assert(result, "Test matrix multiplication", "Found:\n"+test3.print()+"\nExpected:\n"+ref3.print()); test_value(test3.rows(), g_rows, "Test number of rows of result matrix"); test_value(test3.columns(), g_rows, "Test number of columns of result matrix"); // Test incompatible matrix multiplication test_try("Test incompatible matrix multiplication"); try { GMatrixSparse test4 = m_bigger * m_test; test_try_failure("Expected GException::matrix_mismatch exception."); } catch (GException::matrix_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Test another incompatible matrix multiplication test_try("Test incompatible matrix multiplication"); try { GMatrixSparse test5 = m_bigger * m_test; test_try_failure("Expected GException::matrix_mismatch exception."); } catch (GException::matrix_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Return return; }
/***********************************************************************//** * @brief Test matrix arithmetics * * Tests matrix arithmetics. ***************************************************************************/ void TestGMatrixSparse::matrix_arithmetics(void) { // -GMatrixSparse GMatrixSparse test = -m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, -1.0, 0.0), "Test -GMatrixSparse", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse += GMatrixSparse test = m_test; test += m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 2.0, 0.0), "Test GMatrixSparse += GMatrixSparse", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse -= GMatrixSparse test = m_test; test -= m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 0.0, 0.0), "Test GMatrixSparse -= GMatrixSparse", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse *= 3.0 test = m_test; test *= 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 3.0, 0.0), "Test GMatrixSparse *= 3.0", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse /= 3.0 test = m_test; test /= 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 1.0/3.0, 0.0), "Test GMatrixSparse /= 3.0", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse + GMatrixSparse test = m_test + m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 2.0, 0.0), "Test GMatrixSparse + GMatrixSparse", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse - GMatrixSparse test = m_test - m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 0.0, 0.0), "Test GMatrixSparse - GMatrixSparse", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse * 3.0 test = m_test * 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 3.0, 0.0), "Test GMatrixSparse * 3.0", "Unexpected result matrix:\n"+test.print()); // 3.0 * GMatrixSparse test = 3.0 * m_test; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 3.0, 0.0), "Test 3.0 * GMatrixSparse", "Unexpected result matrix:\n"+test.print()); // GMatrixSparse / 3.0 test = m_test / 3.0; test_assert(check_matrix(m_test), "Test source matrix"); test_assert(check_matrix(test, 1.0/3.0, 0.0), "Test GMatrixSparse / 3.0", "Unexpected result matrix:\n"+test.print()); // Test invalid matrix addition test_try("Test invalid matrix addition"); try { test = m_test; test += m_bigger; test_try_failure("Expected GException::matrix_mismatch exception."); } catch (GException::matrix_mismatch &e) { test_try_success(); } catch (std::exception &e) { test_try_failure(e); } // Return return; }
int main(int argc, char *argv[]) { int j, k, noproc, me_no; double sum; double t1, t2; pthread_t *threads; pthread_attr_t pthread_custom_attr; parm *arg; int n, i; for (i = 0; i < NDIM; i++) for (j = 0; j < NDIM; j++) { a[i][j] = i + j; b[i][j] = i + j; } if (argc != 2) { printf("Usage: %s n\n where n is no. of thread\n", argv[0]); exit(1); } n = atoi(argv[1]); if ((n < 1) || (n > MAX_THREAD)) { printf("The no of thread should between 1 and %d.\n", MAX_THREAD); exit(1); } threads = (pthread_t *) malloc(n * sizeof(pthread_t)); pthread_attr_init(&pthread_custom_attr); arg=(parm *)malloc(sizeof(parm)*n); /* setup barrier */ /* Start up thread */ /* Spawn thread */ for (i = 0; i < n; i++) { arg[i].id = i; arg[i].noproc = n; arg[i].dim = NDIM; arg[i].a = &a; arg[i].b = &b; arg[i].c = &c; pthread_create(&threads[i], &pthread_custom_attr, worker, (void *)(arg+i)); } for (i = 0; i < n; i++) { pthread_join(threads[i], NULL); } /* print_matrix(NDIM); */ check_matrix(NDIM); free(arg); return 0; }