private void calculate_grid_info(Locationwp loc, ref grid_info info)
{
// grids start on integer degrees. This makes storing terrain data
// on the SD card a bit easier
info.lat_degrees = (int8_t)((loc.lat < 0 ? (loc.lat - 9999999L) : loc.lat) / (10 * 1000 * 1000L));
info.lon_degrees = (int16_t)((loc.lng < 0 ? (loc.lng - 9999999L) : loc.lng) / (10 * 1000 * 1000L));
// create reference position for this rounded degree position
Locationwp ref1 = new Locationwp();
ref1.lat = info.lat_degrees * 10 * 1000 * 1000L;
ref1.lng = info.lon_degrees * 10 * 1000 * 1000L;
// find offset from reference
Vector3 offset = location_diff(ref1, loc);
// get indices in terms of grid_spacing elements
uint32_t idx_x = (uint32_t)(offset.x / grid_spacing);
uint32_t idx_y = (uint32_t)(offset.y / grid_spacing);
// find indexes into 32*28 grids for this degree reference. Note
// the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square
// overlap between grids
info.grid_idx_x = (ushort)(idx_x / TERRAIN_GRID_BLOCK_SPACING_X);
info.grid_idx_y = (ushort)(idx_y / TERRAIN_GRID_BLOCK_SPACING_Y);
// find the indices within the 32*28 grid
info.idx_x = (byte)(idx_x % TERRAIN_GRID_BLOCK_SPACING_X);
info.idx_y = (byte)(idx_y % TERRAIN_GRID_BLOCK_SPACING_Y);
// find the fraction (0..1) within the square
info.frac_x = (float)((offset.x - idx_x * grid_spacing) / grid_spacing);
info.frac_y = (float)((offset.y - idx_y * grid_spacing) / grid_spacing);
// calculate lat/lon of SW corner of 32*28 grid_block
location_offset(ref1,
info.grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing,
info.grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing);
info.grid_lat = (int32_t)ref1.lat;
info.grid_lon = (int32_t)ref1.lng;
//ASSERT_RANGE(info.idx_x,0, TERRAIN_GRID_BLOCK_SPACING_X-1);
//ASSERT_RANGE(info.idx_y,0, TERRAIN_GRID_BLOCK_SPACING_Y-1);
//ASSERT_RANGE(info.frac_x,0,1);
//ASSERT_RANGE(info.frac_y,0,1);
}
public void test()
{
var fs = File.Open(@"C:\Users\michael\Documents\Mission Planner\sitl\d0\terrain\S36E149.DAT", FileMode.Open);
var br = new BinaryReader(fs);
var buffer = new byte[2048];
while (br.BaseStream.Position < br.BaseStream.Length)
{
grid_info info = new grid_info();
var loc = new Locationwp()
{
lat = -35.363261 * 1.0e7, lng = 149.165230 * 1.0e7
};
calculate_grid_info(loc, ref info);
disk_block.block.lat = info.grid_lat;
disk_block.block.lon = info.grid_lon;
disk_block.block.spacing = grid_spacing;
disk_block.block.grid_idx_x = info.grid_idx_x;
disk_block.block.grid_idx_y = info.grid_idx_y;
disk_block.block.lat_degrees = info.lat_degrees;
disk_block.block.lon_degrees = info.lon_degrees;
disk_block.block.version = TERRAIN_GRID_FORMAT_VERSION;
// 29,5 = 1792000
//read_block(fs);
//continue;
var off = br.BaseStream.Position;
br.Read(buffer, 0, buffer.Length);
disk_block = buffer.ByteArrayToStructure <grid_io_block>(0);
if (disk_block.block.bitmap > 0 && disk_block.block.lat != 0 && disk_block.block.lon != 0)
{
Console.WriteLine("Got {0} - {1} at idx {2},{3} off {4}", disk_block.block.lat, disk_block.block.lon, disk_block.block.grid_idx_x, disk_block.block.grid_idx_y, off);
}
}
}
