/***********************************************************************//**
* @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;
}
