void setup()
{
/* Seed the random number generator with a specific seed for our test. */
srandom(1);
/* Create a new pool and fill it with a sequence of ints. */
test_pool_seq = memory_pool_new(50, sizeof(s32));
s32 *x;
for (u32 i=0; i<22; i++) {
x = (s32 *)memory_pool_add(test_pool_seq);
fail_unless(x != 0, "Null pointer returned by memory_pool_add");
*x = i;
}
/* Create a new pool and fill it entirely with random numbers. */
test_pool_random = memory_pool_new(20, sizeof(s32));
for (u32 i=0; i<20; i++) {
x = (s32 *)memory_pool_add(test_pool_random);
fail_unless(x != 0, "Null pointer returned by memory_pool_add");
*x = sizerand(16);
}
/* Create a new pool and leave it empty. */
test_pool_empty = memory_pool_new(50, sizeof(s32));
}
void dgnss_init_known_baseline(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], double b[3])
{
double ref_ecef[3];
ref_ecef[0] = receiver_ecef[0] + 0.5 * b[0];
ref_ecef[1] = receiver_ecef[1] + 0.5 * b[1];
ref_ecef[2] = receiver_ecef[2] + 0.5 * b[2];
sdiff_t corrected_sdiffs[num_sats];
u8 old_prns[MAX_CHANNELS];
memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8));
//rebase globals to a new reference sat (permutes corrected_sdiffs accordingly)
dgnss_rebase_ref(num_sats, sdiffs, ref_ecef, old_prns, corrected_sdiffs);
double dds[2*(num_sats-1)];
make_measurements(num_sats-1, corrected_sdiffs, dds);
double DE[(num_sats-1)*3];
assign_de_mtx(num_sats, corrected_sdiffs, ref_ecef, DE);
dgnss_reset_iar();
memcpy(&ambiguity_test.sats, &sats_management, sizeof(sats_management));
hypothesis_t *hyp = (hypothesis_t *)memory_pool_add(ambiguity_test.pool);
hyp->ll = 0;
amb_from_baseline(num_sats, DE, dds, b, hyp->N);
double obs_cov[(num_sats-1) * (num_sats-1) * 4];
memset(obs_cov, 0, (num_sats-1) * (num_sats-1) * 4 * sizeof(double));
u8 num_dds = num_sats-1;
for (u8 i=0; i<num_dds; i++) {
for (u8 j=0; j<num_dds; j++) {
u8 i_ = i+num_dds;
u8 j_ = j+num_dds;
if (i==j) {
obs_cov[i*2*num_dds + j] = dgnss_settings.phase_var_test * 2;
obs_cov[i_*2*num_dds + j_] = dgnss_settings.code_var_test * 2;
}
else {
obs_cov[i*2*num_dds + j] = dgnss_settings.phase_var_test;
obs_cov[i_*2*num_dds + j_] = dgnss_settings.code_var_test;
}
}
}
init_residual_matrices(&ambiguity_test.res_mtxs, num_sats-1, DE, obs_cov);
/*printf("Known Base: [");*/
/*for (u8 i=0; i<num_sats-1; i++)*/
/*printf("%d, ", hyp->N[i]);*/
/*printf("]\n");*/
}
/**
* intialize battle struct and all armies
*/
Battle *battle_create (int num_defenders, int num_attackers)
{
Battle *battle = xcalloc(1, sizeof (Battle));
int i;
battle->size_defenders = num_defenders;
battle->size_attackers = num_attackers;
/////////////////////////// create unit arrays /////////////////////////////
battle->defenders = xcalloc(num_defenders, sizeof (Army));
for (i = 0; i < num_defenders; ++i)
army_init(&battle->defenders[i]);
battle->attackers = xcalloc(num_attackers, sizeof (Army));
for (i = 0; i < num_attackers; ++i)
army_init(&battle->attackers[i]);
return memory_pool_add(battle, (void (*)(void *)) battle_free);
}
int symbols_append(struct _symbols *symbols, char *name, int address)
{
int token_len;
struct _memory_pool *memory_pool = symbols->memory_pool;
if (symbols->locked == 1) return 0;
if (symbols_find(symbols, name) != NULL)
{
printf("Error: Label '%s' already defined.\n", name);
return -1;
}
token_len = strlen(name) + 1;
// Check if size of new label is bigger than 255.
if (token_len > 255)
{
printf("Error: Label '%s' is too big.\n", name);
return -1;
}
// If we have no pool, add one.
if (memory_pool == NULL)
{
memory_pool = memory_pool_add((struct _naken_heap *)symbols, SYMBOLS_HEAP_SIZE);
}
// Find a pool that has enough area at the end to add this address.
// If none can be found, alloc a new one.
while(1)
{
if (memory_pool->ptr + token_len + sizeof(struct _symbols_data) < memory_pool->len)
{
break;
}
if (memory_pool->next == NULL)
{
memory_pool->next = memory_pool_add((struct _naken_heap *)symbols, SYMBOLS_HEAP_SIZE);
}
memory_pool = memory_pool->next;
}
// Divide by bytes_per_address (for AVR8 and dsPIC).
//address = address / asm_context->bytes_per_address;
// Set the new label/address entry.
struct _symbols_data *symbols_data =
(struct _symbols_data *)(memory_pool->buffer + memory_pool->ptr);
memcpy(symbols_data->name, name, token_len);
symbols_data->len = token_len;
symbols_data->flag_rw = 0;
symbols_data->flag_export = 0;
symbols_data->address = address;
memory_pool->ptr += token_len + sizeof(struct _symbols_data);
return 0;
}
