const char* opcodeName(Opcode opcode) {
return OpInfo[uint16_t(opcode)].name;
}
int _main_(int /*_argc*/, char** /*_argv*/)
{
uint32_t width = 1280;
uint32_t height = 720;
uint32_t debug = BGFX_DEBUG_TEXT;
uint32_t reset = BGFX_RESET_VSYNC;
bgfx::init();
bgfx::reset(width, height, reset);
// Enable debug text.
bgfx::setDebug(debug);
// Set clear color palette for index 0
bgfx::setClearColor(0, UINT32_C(0x00000000) );
// Set clear color palette for index 1
bgfx::setClearColor(1, UINT32_C(0x303030ff) );
// Set geometry pass view clear state.
bgfx::setViewClear(RENDER_PASS_GEOMETRY_ID
, BGFX_CLEAR_COLOR|BGFX_CLEAR_DEPTH
, 1.0f
, 0
, 1
);
// Set light pass view clear state.
bgfx::setViewClear(RENDER_PASS_LIGHT_ID
, BGFX_CLEAR_COLOR|BGFX_CLEAR_DEPTH
, 1.0f
, 0
, 0
);
// Create vertex stream declaration.
PosNormalTangentTexcoordVertex::init();
PosTexCoord0Vertex::init();
DebugVertex::init();
calcTangents(s_cubeVertices
, BX_COUNTOF(s_cubeVertices)
, PosNormalTangentTexcoordVertex::ms_decl
, s_cubeIndices
, BX_COUNTOF(s_cubeIndices)
);
// Create static vertex buffer.
bgfx::VertexBufferHandle vbh = bgfx::createVertexBuffer(
bgfx::makeRef(s_cubeVertices, sizeof(s_cubeVertices) )
, PosNormalTangentTexcoordVertex::ms_decl
);
// Create static index buffer.
bgfx::IndexBufferHandle ibh = bgfx::createIndexBuffer(bgfx::makeRef(s_cubeIndices, sizeof(s_cubeIndices) ) );
// Create texture sampler uniforms.
bgfx::UniformHandle s_texColor = bgfx::createUniform("s_texColor", bgfx::UniformType::Uniform1iv);
bgfx::UniformHandle s_texNormal = bgfx::createUniform("s_texNormal", bgfx::UniformType::Uniform1iv);
bgfx::UniformHandle s_albedo = bgfx::createUniform("s_albedo", bgfx::UniformType::Uniform1iv);
bgfx::UniformHandle s_normal = bgfx::createUniform("s_normal", bgfx::UniformType::Uniform1iv);
bgfx::UniformHandle s_depth = bgfx::createUniform("s_depth", bgfx::UniformType::Uniform1iv);
bgfx::UniformHandle s_light = bgfx::createUniform("s_light", bgfx::UniformType::Uniform1iv);
bgfx::UniformHandle u_mtx = bgfx::createUniform("u_mtx", bgfx::UniformType::Uniform4x4fv);
bgfx::UniformHandle u_lightPosRadius = bgfx::createUniform("u_lightPosRadius", bgfx::UniformType::Uniform4fv);
bgfx::UniformHandle u_lightRgbInnerR = bgfx::createUniform("u_lightRgbInnerR", bgfx::UniformType::Uniform4fv);
// Create program from shaders.
bgfx::ProgramHandle geomProgram = loadProgram("vs_deferred_geom", "fs_deferred_geom");
bgfx::ProgramHandle lightProgram = loadProgram("vs_deferred_light", "fs_deferred_light");
bgfx::ProgramHandle combineProgram = loadProgram("vs_deferred_combine", "fs_deferred_combine");
bgfx::ProgramHandle debugProgram = loadProgram("vs_deferred_debug", "fs_deferred_debug");
bgfx::ProgramHandle lineProgram = loadProgram("vs_deferred_debug_line", "fs_deferred_debug_line");
// Load diffuse texture.
bgfx::TextureHandle textureColor = loadTexture("fieldstone-rgba.dds");
// Load normal texture.
bgfx::TextureHandle textureNormal = loadTexture("fieldstone-n.dds");
bgfx::TextureHandle gbufferTex[3] = { BGFX_INVALID_HANDLE, BGFX_INVALID_HANDLE, BGFX_INVALID_HANDLE };
bgfx::FrameBufferHandle gbuffer = BGFX_INVALID_HANDLE;
bgfx::FrameBufferHandle lightBuffer = BGFX_INVALID_HANDLE;
// Imgui.
imguiCreate();
const int64_t timeOffset = bx::getHPCounter();
const bgfx::RendererType::Enum renderer = bgfx::getRendererType();
const float texelHalf = bgfx::RendererType::Direct3D9 == renderer ? 0.5f : 0.0f;
s_originBottomLeft = bgfx::RendererType::OpenGL == renderer || bgfx::RendererType::OpenGLES == renderer;
// Get renderer capabilities info.
const bgfx::Caps* caps = bgfx::getCaps();
uint32_t oldWidth = 0;
uint32_t oldHeight = 0;
uint32_t oldReset = reset;
int32_t scrollArea = 0;
int32_t numLights = 512;
float lightAnimationSpeed = 0.3f;
bool animateMesh = true;
bool showScissorRects = false;
bool showGBuffer = true;
float view[16];
float initialPos[3] = { 0.0f, 0.0f, -15.0f };
cameraCreate();
cameraSetPosition(initialPos);
cameraSetVerticalAngle(0.0f);
cameraGetViewMtx(view);
entry::MouseState mouseState;
while (!entry::processEvents(width, height, debug, reset, &mouseState) )
{
int64_t now = bx::getHPCounter();
static int64_t last = now;
const int64_t frameTime = now - last;
last = now;
const double freq = double(bx::getHPFrequency() );
const double toMs = 1000.0/freq;
const float deltaTime = float(frameTime/freq);
float time = (float)( (now-timeOffset)/freq);
// Use debug font to print information about this example.
bgfx::dbgTextClear();
bgfx::dbgTextPrintf(0, 1, 0x4f, "bgfx/examples/21-deferred");
bgfx::dbgTextPrintf(0, 2, 0x6f, "Description: MRT rendering and deferred shading.");
bgfx::dbgTextPrintf(0, 3, 0x0f, "Frame: % 7.3f[ms]", double(frameTime)*toMs);
if (2 > caps->maxFBAttachments)
{
// When multiple render targets (MRT) is not supported by GPU,
// implement alternative code path that doesn't use MRT.
bool blink = uint32_t(time*3.0f)&1;
bgfx::dbgTextPrintf(0, 5, blink ? 0x1f : 0x01, " MRT not supported by GPU. ");
// Set view 0 default viewport.
bgfx::setViewRect(0, 0, 0, width, height);
// This dummy draw call is here to make sure that view 0 is cleared
// if no other draw calls are submitted to view 0.
bgfx::submit(0);
}
else
{
if (oldWidth != width
|| oldHeight != height
|| oldReset != reset
|| !bgfx::isValid(gbuffer) )
{
// Recreate variable size render targets when resolution changes.
oldWidth = width;
oldHeight = height;
oldReset = reset;
if (bgfx::isValid(gbuffer) )
{
bgfx::destroyFrameBuffer(gbuffer);
}
const uint32_t samplerFlags = 0
| BGFX_TEXTURE_RT
| BGFX_TEXTURE_MIN_POINT
| BGFX_TEXTURE_MAG_POINT
| BGFX_TEXTURE_MIP_POINT
| BGFX_TEXTURE_U_CLAMP
| BGFX_TEXTURE_V_CLAMP
;
gbufferTex[0] = bgfx::createTexture2D(width, height, 1, bgfx::TextureFormat::BGRA8, samplerFlags);
gbufferTex[1] = bgfx::createTexture2D(width, height, 1, bgfx::TextureFormat::BGRA8, samplerFlags);
gbufferTex[2] = bgfx::createTexture2D(width, height, 1, bgfx::TextureFormat::D24, samplerFlags);
gbuffer = bgfx::createFrameBuffer(BX_COUNTOF(gbufferTex), gbufferTex, true);
if (bgfx::isValid(lightBuffer) )
{
bgfx::destroyFrameBuffer(lightBuffer);
}
lightBuffer = bgfx::createFrameBuffer(width, height, bgfx::TextureFormat::BGRA8, samplerFlags);
}
imguiBeginFrame(mouseState.m_mx
, mouseState.m_my
, (mouseState.m_buttons[entry::MouseButton::Left ] ? IMGUI_MBUT_LEFT : 0)
| (mouseState.m_buttons[entry::MouseButton::Right ] ? IMGUI_MBUT_RIGHT : 0)
, 0
, width
, height
);
imguiBeginScrollArea("Settings", width - width / 5 - 10, 10, width / 5, height / 3, &scrollArea);
imguiSeparatorLine();
imguiSlider("Num lights", numLights, 1, 2048);
if (imguiCheck("Show G-Buffer.", showGBuffer) )
{
showGBuffer = !showGBuffer;
}
if (imguiCheck("Show light scissor.", showScissorRects) )
{
showScissorRects = !showScissorRects;
}
if (imguiCheck("Animate mesh.", animateMesh) )
{
animateMesh = !animateMesh;
}
imguiSlider("Lights animation speed", lightAnimationSpeed, 0.0f, 0.4f, 0.01f);
imguiEndScrollArea();
imguiEndFrame();
// Update camera.
cameraUpdate(deltaTime, mouseState);
cameraGetViewMtx(view);
// Setup views
float vp[16];
float invMvp[16];
{
bgfx::setViewRect(RENDER_PASS_GEOMETRY_ID, 0, 0, width, height);
bgfx::setViewRect(RENDER_PASS_LIGHT_ID, 0, 0, width, height);
bgfx::setViewRect(RENDER_PASS_COMBINE_ID, 0, 0, width, height);
bgfx::setViewRect(RENDER_PASS_DEBUG_LIGHTS_ID, 0, 0, width, height);
bgfx::setViewRect(RENDER_PASS_DEBUG_GBUFFER_ID, 0, 0, width, height);
bgfx::setViewFrameBuffer(RENDER_PASS_LIGHT_ID, lightBuffer);
float proj[16];
mtxProj(proj, 60.0f, float(width)/float(height), 0.1f, 100.0f);
bgfx::setViewFrameBuffer(RENDER_PASS_GEOMETRY_ID, gbuffer);
bgfx::setViewTransform(RENDER_PASS_GEOMETRY_ID, view, proj);
bx::mtxMul(vp, view, proj);
bx::mtxInverse(invMvp, vp);
bx::mtxOrtho(proj, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 100.0f);
bgfx::setViewTransform(RENDER_PASS_LIGHT_ID, NULL, proj);
bgfx::setViewTransform(RENDER_PASS_COMBINE_ID, NULL, proj);
const float aspectRatio = float(height)/float(width);
const float size = 10.0f;
bx::mtxOrtho(proj, -size, size, size*aspectRatio, -size*aspectRatio, 0.0f, 1000.0f);
bgfx::setViewTransform(RENDER_PASS_DEBUG_GBUFFER_ID, NULL, proj);
bx::mtxOrtho(proj, 0.0f, (float)width, 0.0f, (float)height, 0.0f, 1000.0f);
bgfx::setViewTransform(RENDER_PASS_DEBUG_LIGHTS_ID, NULL, proj);
}
const uint32_t dim = 11;
const float offset = (float(dim-1) * 3.0f) * 0.5f;
// Draw into geometry pass.
for (uint32_t yy = 0; yy < dim; ++yy)
{
for (uint32_t xx = 0; xx < dim; ++xx)
{
float mtx[16];
if (animateMesh)
{
bx::mtxRotateXY(mtx, time*1.023f + xx*0.21f, time*0.03f + yy*0.37f);
}
else
{
bx::mtxIdentity(mtx);
}
mtx[12] = -offset + float(xx)*3.0f;
mtx[13] = -offset + float(yy)*3.0f;
mtx[14] = 0.0f;
// Set transform for draw call.
bgfx::setTransform(mtx);
// Set vertex and fragment shaders.
bgfx::setProgram(geomProgram);
// Set vertex and index buffer.
bgfx::setVertexBuffer(vbh);
bgfx::setIndexBuffer(ibh);
// Bind textures.
bgfx::setTexture(0, s_texColor, textureColor);
bgfx::setTexture(1, s_texNormal, textureNormal);
// Set render states.
bgfx::setState(0
| BGFX_STATE_RGB_WRITE
| BGFX_STATE_ALPHA_WRITE
| BGFX_STATE_DEPTH_WRITE
| BGFX_STATE_DEPTH_TEST_LESS
| BGFX_STATE_MSAA
);
// Submit primitive for rendering to view 0.
bgfx::submit(RENDER_PASS_GEOMETRY_ID);
}
}
// Draw lights into light buffer.
for (int32_t light = 0; light < numLights; ++light)
{
Sphere lightPosRadius;
float lightTime = time * lightAnimationSpeed * (sin(light/float(numLights) * bx::piHalf ) * 0.5f + 0.5f);
lightPosRadius.m_center[0] = sin( ( (lightTime + light*0.47f) + bx::piHalf*1.37f ) )*offset;
lightPosRadius.m_center[1] = cos( ( (lightTime + light*0.69f) + bx::piHalf*1.49f ) )*offset;
lightPosRadius.m_center[2] = sin( ( (lightTime + light*0.37f) + bx::piHalf*1.57f ) )*2.0f;
lightPosRadius.m_radius = 2.0f;
Aabb aabb;
sphereToAabb(aabb, lightPosRadius);
float box[8][3] =
{
{ aabb.m_min[0], aabb.m_min[1], aabb.m_min[2] },
{ aabb.m_min[0], aabb.m_min[1], aabb.m_max[2] },
{ aabb.m_min[0], aabb.m_max[1], aabb.m_min[2] },
{ aabb.m_min[0], aabb.m_max[1], aabb.m_max[2] },
{ aabb.m_max[0], aabb.m_min[1], aabb.m_min[2] },
{ aabb.m_max[0], aabb.m_min[1], aabb.m_max[2] },
{ aabb.m_max[0], aabb.m_max[1], aabb.m_min[2] },
{ aabb.m_max[0], aabb.m_max[1], aabb.m_max[2] },
};
float xyz[3];
bx::vec3MulMtxH(xyz, box[0], vp);
float minx = xyz[0];
float miny = xyz[1];
float maxx = xyz[0];
float maxy = xyz[1];
float maxz = xyz[2];
for (uint32_t ii = 1; ii < 8; ++ii)
{
bx::vec3MulMtxH(xyz, box[ii], vp);
minx = bx::fmin(minx, xyz[0]);
miny = bx::fmin(miny, xyz[1]);
maxx = bx::fmax(maxx, xyz[0]);
maxy = bx::fmax(maxy, xyz[1]);
maxz = bx::fmax(maxz, xyz[2]);
}
// Cull light if it's fully behind camera.
if (maxz >= 0.0f)
{
float x0 = bx::fclamp( (minx * 0.5f + 0.5f) * width, 0.0f, (float)width);
float y0 = bx::fclamp( (miny * 0.5f + 0.5f) * height, 0.0f, (float)height);
float x1 = bx::fclamp( (maxx * 0.5f + 0.5f) * width, 0.0f, (float)width);
float y1 = bx::fclamp( (maxy * 0.5f + 0.5f) * height, 0.0f, (float)height);
if (showScissorRects)
{
bgfx::TransientVertexBuffer tvb;
bgfx::TransientIndexBuffer tib;
if (bgfx::allocTransientBuffers(&tvb, DebugVertex::ms_decl, 4, &tib, 8) )
{
uint32_t abgr = 0x8000ff00;
DebugVertex* vertex = (DebugVertex*)tvb.data;
vertex->m_x = x0;
vertex->m_y = y0;
vertex->m_z = 0.0f;
vertex->m_abgr = abgr;
++vertex;
vertex->m_x = x1;
vertex->m_y = y0;
vertex->m_z = 0.0f;
vertex->m_abgr = abgr;
++vertex;
vertex->m_x = x1;
vertex->m_y = y1;
vertex->m_z = 0.0f;
vertex->m_abgr = abgr;
++vertex;
vertex->m_x = x0;
vertex->m_y = y1;
vertex->m_z = 0.0f;
vertex->m_abgr = abgr;
uint16_t* indices = (uint16_t*)tib.data;
*indices++ = 0;
*indices++ = 1;
*indices++ = 1;
*indices++ = 2;
*indices++ = 2;
*indices++ = 3;
*indices++ = 3;
*indices++ = 0;
bgfx::setProgram(lineProgram);
bgfx::setVertexBuffer(&tvb);
bgfx::setIndexBuffer(&tib);
bgfx::setState(0
| BGFX_STATE_RGB_WRITE
| BGFX_STATE_PT_LINES
| BGFX_STATE_BLEND_ALPHA
);
bgfx::submit(RENDER_PASS_DEBUG_LIGHTS_ID);
}
}
uint8_t val = light&7;
float lightRgbInnerR[4] =
{
val & 0x1 ? 1.0f : 0.25f,
val & 0x2 ? 1.0f : 0.25f,
val & 0x4 ? 1.0f : 0.25f,
0.8f,
};
// Draw light.
bgfx::setUniform(u_lightPosRadius, &lightPosRadius);
bgfx::setUniform(u_lightRgbInnerR, lightRgbInnerR);
bgfx::setUniform(u_mtx, invMvp);
const uint16_t scissorHeight = uint16_t(y1-y0);
bgfx::setScissor(uint16_t(x0), height-scissorHeight-uint16_t(y0), uint16_t(x1-x0), scissorHeight);
bgfx::setTexture(0, s_normal, gbuffer, 1);
bgfx::setTexture(1, s_depth, gbuffer, 2);
bgfx::setProgram(lightProgram);
bgfx::setState(0
| BGFX_STATE_RGB_WRITE
| BGFX_STATE_ALPHA_WRITE
| BGFX_STATE_BLEND_ADD
);
screenSpaceQuad( (float)width, (float)height, texelHalf, s_originBottomLeft);
bgfx::submit(RENDER_PASS_LIGHT_ID);
}
}
// Combine color and light buffers.
bgfx::setTexture(0, s_albedo, gbuffer, 0);
bgfx::setTexture(1, s_light, lightBuffer, 0);
bgfx::setProgram(combineProgram);
bgfx::setState(0
| BGFX_STATE_RGB_WRITE
| BGFX_STATE_ALPHA_WRITE
);
screenSpaceQuad( (float)width, (float)height, texelHalf, s_originBottomLeft);
bgfx::submit(RENDER_PASS_COMBINE_ID);
if (showGBuffer)
{
const float aspectRatio = float(width)/float(height);
// Draw debug GBuffer.
for (uint32_t ii = 0; ii < BX_COUNTOF(gbufferTex); ++ii)
{
float mtx[16];
bx::mtxSRT(mtx
, aspectRatio, 1.0f, 1.0f
, 0.0f, 0.0f, 0.0f
, -7.9f - BX_COUNTOF(gbufferTex)*0.1f*0.5f + ii*2.1f*aspectRatio, 4.0f, 0.0f
);
bgfx::setTransform(mtx);
bgfx::setProgram(debugProgram);
bgfx::setVertexBuffer(vbh);
bgfx::setIndexBuffer(ibh, 0, 6);
bgfx::setTexture(0, s_texColor, gbufferTex[ii]);
bgfx::setState(BGFX_STATE_RGB_WRITE);
bgfx::submit(RENDER_PASS_DEBUG_GBUFFER_ID);
}
}
}
// Advance to next frame. Rendering thread will be kicked to
// process submitted rendering primitives.
bgfx::frame();
}
// Cleanup.
cameraDestroy();
imguiDestroy();
if (bgfx::isValid(gbuffer) )
{
bgfx::destroyFrameBuffer(gbuffer);
bgfx::destroyFrameBuffer(lightBuffer);
}
bgfx::destroyIndexBuffer(ibh);
bgfx::destroyVertexBuffer(vbh);
bgfx::destroyProgram(geomProgram);
bgfx::destroyProgram(lightProgram);
bgfx::destroyProgram(combineProgram);
bgfx::destroyProgram(debugProgram);
bgfx::destroyProgram(lineProgram);
bgfx::destroyTexture(textureColor);
bgfx::destroyTexture(textureNormal);
bgfx::destroyUniform(s_texColor);
bgfx::destroyUniform(s_texNormal);
bgfx::destroyUniform(s_albedo);
bgfx::destroyUniform(s_normal);
bgfx::destroyUniform(s_depth);
bgfx::destroyUniform(s_light);
bgfx::destroyUniform(u_lightPosRadius);
bgfx::destroyUniform(u_lightRgbInnerR);
bgfx::destroyUniform(u_mtx);
// Shutdown bgfx.
bgfx::shutdown();
return 0;
}
// *****************************************************************************
// Main
int main(int argc, char* const argv[])
{
try {
if (argc != 2) {
std::cout << "Usage: " << argv[0] << " file\n";
return 1;
}
std::string file(argv[1]);
Exiv2::Image::AutoPtr image = Exiv2::ImageFactory::open(file);
assert (image.get() != 0);
image->readMetadata();
Exiv2::ExifData &ed = image->exifData();
if (ed.empty()) {
std::string error = file + ": No Exif data found in the file";
throw Exiv2::Error(1, error);
}
std::cout << "Copy construction, non-intrusive changes\n";
Exiv2::ExifData ed1(ed);
ed1["Exif.Image.DateTime"] = "Sunday, 11am";
ed1["Exif.Image.Orientation"] = uint16_t(2);
ed1["Exif.Photo.DateTimeOriginal"] = "Sunday, 11am";
ed1["Exif.Photo.MeteringMode"] = uint16_t(1);
ed1["Exif.Iop.InteroperabilityIndex"] = "123";
// ed1["Exif.Thumbnail.Orientation"] = uint16_t(2);
write(file, ed1);
print(file);
std::cout << "----------------------------------------------\n";
std::cout << "Copy construction, intrusive changes\n";
Exiv2::ExifData ed2(ed);
ed2["Exif.Image.DateTime"] = "Sunday, 11am and ten minutes";
ed2["Exif.Image.Orientation"] = "2 3 4 5";
ed2["Exif.Photo.DateTimeOriginal"] = "Sunday, 11am and ten minutes";
ed2["Exif.Photo.MeteringMode"] = "1 2 3 4 5 6";
ed2["Exif.Iop.InteroperabilityIndex"] = "1234";
ed2["Exif.Thumbnail.Orientation"] = "2 3 4 5 6";
write(file, ed2);
print(file);
std::cout << "----------------------------------------------\n";
std::cout << "Assignment, non-intrusive changes\n";
Exiv2::ExifData ed3;
ed3["Exif.Iop.InteroperabilityVersion"] = "Test 6 Iop tag";
ed3["Exif.Thumbnail.Artist"] = "Test 6 Ifd1 tag";
ed3 = ed;
ed3["Exif.Image.DateTime"] = "Sunday, 11am";
ed3["Exif.Image.Orientation"] = uint16_t(2);
ed3["Exif.Photo.DateTimeOriginal"] = "Sunday, 11am";
ed3["Exif.Photo.MeteringMode"] = uint16_t(1);
ed3["Exif.Iop.InteroperabilityIndex"] = "123";
// ed3["Exif.Thumbnail.Orientation"] = uint16_t(2);
write(file, ed3);
print(file);
std::cout << "----------------------------------------------\n";
std::cout << "Assignment, intrusive changes\n";
Exiv2::ExifData ed4;
ed4["Exif.Iop.InteroperabilityVersion"] = "Test 6 Iop tag";
ed4["Exif.Thumbnail.Artist"] = "Test 6 Ifd1 tag";
ed4 = ed;
ed4["Exif.Image.DateTime"] = "Sunday, 11am and ten minutes";
ed4["Exif.Image.Orientation"] = "2 3 4 5";
ed4["Exif.Photo.DateTimeOriginal"] = "Sunday, 11am and ten minutes";
ed4["Exif.Photo.MeteringMode"] = uint16_t(1);
ed4["Exif.Iop.InteroperabilityIndex"] = "123";
ed4["Exif.Thumbnail.Orientation"] = uint16_t(2);
write(file, ed4);
print(file);
return 0;
}
catch (Exiv2::AnyError& e) {
std::cout << "Caught Exiv2 exception '" << e << "'\n";
return -1;
}
}
Status* InterpreterCodeImpl::execute(std::vector<Var*>& vars) {
ByteStack stack;
typedef std::map<uint16_t, ByteStorage> VarMap;
VarMap var_map;
for (std::vector<Var*>::iterator it = vars.begin(); it != vars.end(); ++it) {
if ((*it)->name() == "#__INTERPRETER_TRACING__#") {
m_trace = true;
}
}
BytecodeFunction* function = (BytecodeFunction*) functionById(0);
Bytecode* bytecode = function->bytecode();
for (size_t bci = 0; bci < bytecode->length();) {
Instruction insn = BC_INVALID;
size_t length = 1; // size of the BC_INVALID
decodeInsn(bytecode, bci, insn, length);
switch (insn) {
case BC_INVALID:
return new Status("BC_INVALID", bci);
break;
case BC_DLOAD:
stack.pushTyped(bytecode->getDouble(bci + 1));
break;
case BC_ILOAD:
stack.pushTyped(bytecode->getInt64(bci + 1));
break;
case BC_SLOAD:
stack.pushTyped(bytecode->getUInt16(bci + 1));
break;
case BC_DLOAD0:
stack.pushTyped(double(0));
break;
case BC_ILOAD0:
stack.pushTyped(int64_t(0));
break;
case BC_SLOAD0:
stack.pushTyped(uint16_t(0));
break;
case BC_DLOAD1:
stack.pushTyped(double(1));
break;
case BC_ILOAD1:
stack.pushTyped(int64_t(1));
break;
case BC_DADD:
stack.pushDouble(stack.popDouble() + stack.popDouble());
break;
case BC_IADD:
stack.pushInt64(stack.popInt64() + stack.popInt64());
break;
case BC_DSUB:
stack.pushDouble(stack.popDouble() - stack.popDouble());
break;
case BC_ISUB:
stack.pushInt64(stack.popInt64() - stack.popInt64());
break;
case BC_DMUL:
stack.pushDouble(stack.popDouble() * stack.popDouble());
break;
case BC_IMUL:
stack.pushInt64(stack.popInt64() * stack.popInt64());
break;
case BC_DDIV:
stack.pushDouble(stack.popDouble() / stack.popDouble());
break;
case BC_IDIV:
stack.pushInt64(stack.popInt64() / stack.popInt64());
break;
case BC_IMOD:
stack.pushInt64(stack.popInt64() % stack.popInt64());
break;
case BC_DNEG:
stack.pushDouble(-stack.popDouble());
break;
case BC_INEG:
stack.pushInt64(-stack.popInt64());
break;
case BC_IPRINT:
std::cout << stack.popInt64();
break;
case BC_DPRINT:
std::cout << stack.popDouble();
break;
case BC_SPRINT:
std::cout << constantById(stack.popUInt16());
break;
case BC_POP:
stack.pop();
break;
case BC_LOADDVAR0:
stack.pushDouble(var_map[0].getDouble());
break;
case BC_LOADDVAR1:
stack.pushDouble(var_map[1].getDouble());
break;
case BC_LOADDVAR2:
stack.pushDouble(var_map[2].getDouble());
break;
case BC_LOADDVAR3:
stack.pushDouble(var_map[3].getDouble());
break;
case BC_LOADIVAR0:
stack.pushInt64(var_map[0].getInt64());
break;
case BC_LOADIVAR1:
stack.pushInt64(var_map[1].getInt64());
break;
case BC_LOADIVAR2:
stack.pushInt64(var_map[2].getInt64());
break;
case BC_LOADIVAR3:
stack.pushInt64(var_map[3].getInt64());
break;
case BC_LOADSVAR0:
stack.pushUInt16(var_map[0].getUInt16());
break;
case BC_LOADSVAR1:
stack.pushUInt16(var_map[1].getUInt16());
break;
case BC_LOADSVAR2:
stack.pushUInt16(var_map[2].getUInt16());
break;
case BC_LOADSVAR3:
stack.pushUInt16(var_map[3].getUInt16());
break;
case BC_STOREDVAR0:
var_map[0].setDouble(stack.popDouble());
break;
case BC_STOREIVAR0:
var_map[0].setInt64(stack.popInt64());
break;
case BC_STORESVAR0:
var_map[0].setUInt16(stack.popUInt16());
break;
case BC_LOADDVAR:
stack.pushDouble(var_map[bytecode->getUInt16(bci + 1)].getDouble());
break;
case BC_LOADIVAR:
stack.pushInt64(var_map[bytecode->getUInt16(bci + 1)].getInt64());
break;
case BC_LOADSVAR:
stack.pushUInt16(var_map[bytecode->getUInt16(bci + 1)].getUInt16());
break;
case BC_STOREDVAR:
var_map[bytecode->getUInt16(bci + 1)].setDouble(stack.popDouble());
break;
case BC_STOREIVAR:
var_map[bytecode->getUInt16(bci + 1)].setInt64(stack.popInt64());
break;
case BC_STORESVAR:
// out << name << " @" << getUInt16(bci + 1);
var_map[bytecode->getUInt16(bci + 1)].setUInt16(stack.popUInt16());
break;
// case BC_LOADCTXDVAR:
// case BC_STORECTXDVAR:
// case BC_LOADCTXIVAR:
// case BC_STORECTXIVAR:
// case BC_LOADCTXSVAR:
// case BC_STORECTXSVAR:
//// out << name << " @" << getUInt16(bci + 1)
//// << ":" << getUInt16(bci + 3);
// break;
case BC_IFICMPNE:
if (stack.popInt64() != stack.popInt64()) {
bci += bytecode->getInt16(bci + 1) + 1;
continue;
}
break;
case BC_IFICMPE:
if (stack.popInt64() == stack.popInt64()) {
bci += bytecode->getInt16(bci + 1) + 1;
continue;
}
break;
case BC_IFICMPG:
if (stack.popInt64() > stack.popInt64()) {
bci += bytecode->getInt16(bci + 1) + 1;
continue;
}
break;
case BC_IFICMPGE:
if (stack.popInt64() >= stack.popInt64()) {
bci += bytecode->getInt16(bci + 1) + 1;
continue;
}
break;
case BC_IFICMPL:
if (stack.popInt64() < stack.popInt64()) {
bci += bytecode->getInt16(bci + 1) + 1;
continue;
}
break;
case BC_IFICMPLE:
if (stack.popInt64() <= stack.popInt64()) {
bci += bytecode->getInt16(bci + 1) + 1;
continue;
}
break;
case BC_JA:
bci += bytecode->getInt16(bci + 1) + 1;
continue;
break;
case BC_CALL://{
stack.pushTyped(bci + length);
stack.pushTyped(function->id());
// std::clog << "saving return address: " << function->id() << ":" << bci + length << std::endl;
// uint16_t f = stack.popUInt16();
// size_t b = stack.popTyped<size_t>();
// std::clog << "checking return address: " << f << ":" << b << std::endl;
// stack.pushTyped(bci + length);
// stack.pushTyped(function->id());
function = (BytecodeFunction*) functionById(bytecode->getUInt16(bci + 1));
if (!function) {
return new Status("Unresolved function ID\n", bci);
}
bytecode = function->bytecode();
bci = 0;
continue;
break;//}
case BC_CALLNATIVE:
return new Status("Native functions are currently not supported\n", bci);
break;
case BC_RETURN: {
uint16_t new_function_id = stack.popUInt16();
// std::clog << "new func id=" << new_function_id << std::endl;
function = (BytecodeFunction*) functionById(new_function_id);
if (!function) {
return new Status("Unresolved function ID\n", bci);
}
bytecode = function->bytecode();
size_t new_bci = stack.popTyped<size_t>();
// std::clog << "new bci=" << new_bci << std::endl;
bci = new_bci;
continue;
break;
}
case BC_BREAK:
return new Status("Breakpoints are currently not supported\n", bci);
break;
default:
return new Status("Unknown or unsupported instruction\n", bci);
}
bci += length;
}
#ifdef ENABLE_TRACING
std::cout << "Result = " << var_map[0].getInt64() << std::endl;
#endif
return 0;
}
// The MediaStreamGraph guarantees that this is actually one block, for
// AudioNodeStreams.
void
AudioNodeStream::ProcessInput(GraphTime aFrom, GraphTime aTo, uint32_t aFlags)
{
uint16_t outputCount = mLastChunks.Length();
MOZ_ASSERT(outputCount == std::max(uint16_t(1), mEngine->OutputCount()));
if (!mIsActive) {
// mLastChunks are already null.
#ifdef DEBUG
for (const auto& chunk : mLastChunks) {
MOZ_ASSERT(chunk.IsNull());
}
#endif
} else if (InMutedCycle()) {
mInputChunks.Clear();
for (uint16_t i = 0; i < outputCount; ++i) {
mLastChunks[i].SetNull(WEBAUDIO_BLOCK_SIZE);
}
} else {
// We need to generate at least one input
uint16_t maxInputs = std::max(uint16_t(1), mEngine->InputCount());
mInputChunks.SetLength(maxInputs);
for (uint16_t i = 0; i < maxInputs; ++i) {
ObtainInputBlock(mInputChunks[i], i);
}
bool finished = false;
if (mPassThrough) {
MOZ_ASSERT(outputCount == 1, "For now, we only support nodes that have one output port");
mLastChunks[0] = mInputChunks[0];
} else {
if (maxInputs <= 1 && outputCount <= 1) {
mEngine->ProcessBlock(this, aFrom,
mInputChunks[0], &mLastChunks[0], &finished);
} else {
mEngine->ProcessBlocksOnPorts(this, mInputChunks, mLastChunks, &finished);
}
}
for (uint16_t i = 0; i < outputCount; ++i) {
NS_ASSERTION(mLastChunks[i].GetDuration() == WEBAUDIO_BLOCK_SIZE,
"Invalid WebAudio chunk size");
}
if (finished) {
mMarkAsFinishedAfterThisBlock = true;
if (mIsActive) {
ScheduleCheckForInactive();
}
}
if (mDisabledTrackIDs.Contains(static_cast<TrackID>(AUDIO_TRACK))) {
for (uint32_t i = 0; i < outputCount; ++i) {
mLastChunks[i].SetNull(WEBAUDIO_BLOCK_SIZE);
}
}
}
if (!mFinished) {
// Don't output anything while finished
if (mFlags & EXTERNAL_OUTPUT) {
AdvanceOutputSegment();
}
if (mMarkAsFinishedAfterThisBlock && (aFlags & ALLOW_FINISH)) {
// This stream was finished the last time that we looked at it, and all
// of the depending streams have finished their output as well, so now
// it's time to mark this stream as finished.
if (mFlags & EXTERNAL_OUTPUT) {
FinishOutput();
}
FinishOnGraphThread();
}
}
}
/* Check for automatic takeoff conditions being met using the following sequence:
* 1) Check for adequate GPS lock - if not return false
* 2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
* 3) Wait until the timer has reached the specified value (increments of 0.1 sec) and then check the GPS speed against the threshold
* 4) If the GPS speed is above the threshold and the attitude is within limits then return true and reset the timer
* 5) If the GPS speed and attitude within limits has not been achieved after 2.5 seconds, return false and reset the timer
* 6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
* NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
*/
bool Plane::auto_takeoff_check(void)
{
// this is a more advanced check that relies on TECS
uint32_t now = millis();
static bool launchTimerStarted;
static uint32_t last_tkoff_arm_time;
static uint32_t last_check_ms;
uint16_t wait_time_ms = min(uint16_t(g.takeoff_throttle_delay)*100,12700);
// Reset states if process has been interrupted
if (last_check_ms && (now - last_check_ms) > 200) {
gcs_send_text_fmt(PSTR("Timer Interrupted AUTO"));
launchTimerStarted = false;
last_tkoff_arm_time = 0;
last_check_ms = now;
return false;
}
last_check_ms = now;
// Check for bad GPS
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
// no auto takeoff without GPS lock
return false;
}
// Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing
if (!launchTimerStarted &&
!is_zero(g.takeoff_throttle_min_accel) &&
SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
goto no_launch;
}
// we've reached the acceleration threshold, so start the timer
if (!launchTimerStarted) {
launchTimerStarted = true;
last_tkoff_arm_time = now;
gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"),
(double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f));
}
// Only perform velocity check if not timed out
if ((now - last_tkoff_arm_time) > wait_time_ms+100U) {
gcs_send_text_fmt(PSTR("Timeout AUTO"));
goto no_launch;
}
// Check aircraft attitude for bad launch
if (ahrs.pitch_sensor <= -3000 ||
ahrs.pitch_sensor >= 4500 ||
abs(ahrs.roll_sensor) > 3000) {
gcs_send_text_fmt(PSTR("Bad Launch AUTO"));
goto no_launch;
}
// Check ground speed and time delay
if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) &&
((now - last_tkoff_arm_time) >= wait_time_ms)) {
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), (double)gps.ground_speed());
launchTimerStarted = false;
last_tkoff_arm_time = 0;
return true;
}
// we're not launching yet, but the timer is still going
return false;
no_launch:
launchTimerStarted = false;
last_tkoff_arm_time = 0;
return false;
}
void setupFrame(bool _doLightAdapt, bool _doBloom, bool _doFxaa)
{
m_doLightAdapt = _doLightAdapt;
m_doBloom = _doBloom;
m_doFxaa = _doFxaa;
float screenProj[16];
float windowProj[16];
bx::mtxOrtho(screenProj, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 100.0f);
bx::mtxOrtho(windowProj, 0.0f, (float)m_width, (float)m_height, 0.0f, 0.0f, 1000.0f);
bgfx::setViewRect(ViewIdSkybox, 0, 0, (uint16_t)m_width, (uint16_t)m_height);
bgfx::setViewRect(ViewIdMesh, 0, 0, (uint16_t)m_width, (uint16_t)m_height);
bgfx::setViewTransform(ViewIdSkybox, NULL, screenProj);
//bgfx::setViewTransform for ViewIdMesh is called in 'setActiveCamera()'.
bgfx::setViewFrameBuffer(ViewIdSkybox, m_fbMain);
bgfx::setViewFrameBuffer(ViewIdMesh, m_fbMain);
bgfx::setViewClear(ViewIdSkybox, BGFX_CLEAR_COLOR|BGFX_CLEAR_DEPTH, 0x303030ff, 1.0f, 0);
if (m_doLightAdapt)
{
bgfx::setViewRect(ViewIdLum0, 0, 0, 256, 256);
bgfx::setViewRect(ViewIdLum1, 0, 0, 64, 64);
bgfx::setViewRect(ViewIdLum2, 0, 0, 16, 16);
bgfx::setViewRect(ViewIdLum3, 0, 0, 4, 4);
bgfx::setViewRect(ViewIdLum4, 0, 0, 1, 1);
bgfx::setViewRect(ViewIdUpdateLastLum, 0, 0, 1, 1);
bgfx::setViewTransform(ViewIdLum0, NULL, screenProj);
bgfx::setViewTransform(ViewIdLum1, NULL, screenProj);
bgfx::setViewTransform(ViewIdLum2, NULL, screenProj);
bgfx::setViewTransform(ViewIdLum3, NULL, screenProj);
bgfx::setViewTransform(ViewIdLum4, NULL, screenProj);
bgfx::setViewTransform(ViewIdUpdateLastLum, NULL, screenProj);
bgfx::setViewFrameBuffer(ViewIdLum0, m_fbLumCalc[0]);
bgfx::setViewFrameBuffer(ViewIdLum1, m_fbLumCalc[1]);
bgfx::setViewFrameBuffer(ViewIdLum2, m_fbLumCalc[2]);
bgfx::setViewFrameBuffer(ViewIdLum3, m_fbLumCalc[3]);
bgfx::setViewFrameBuffer(ViewIdLum4, m_fbLumCur);
bgfx::setViewFrameBuffer(ViewIdUpdateLastLum, m_fbLumLast);
}
if (m_doBloom)
{
bgfx::setViewRect(ViewIdBright, 0, 0, uint16_t(m_brightWidth), uint16_t(m_brightHeight));
bgfx::setViewRect(ViewIdBloom, 0, 0, uint16_t(m_blurWidth), uint16_t(m_blurHeight));
bgfx::setViewTransform(ViewIdBright, NULL, screenProj);
bgfx::setViewTransform(ViewIdBloom, NULL, screenProj);
bgfx::setViewFrameBuffer(ViewIdBright, m_fbBright);
bgfx::setViewFrameBuffer(ViewIdBloom, m_fbBlur);
}
bgfx::setViewRect(ViewIdTonemap, 0, 0, (uint16_t)m_width, (uint16_t)m_height);
bgfx::setViewTransform(ViewIdTonemap, NULL, screenProj);
if (m_doFxaa)
{
bgfx::setViewFrameBuffer(ViewIdTonemap, m_fbPost);
bgfx::setViewRect(ViewIdFxaa, 0, 0, (uint16_t)m_width, (uint16_t)m_height);
bgfx::setViewTransform(ViewIdFxaa, NULL, screenProj);
}
else
{
const bgfx::FrameBufferHandle invalidHandle = BGFX_INVALID_HANDLE;
bgfx::setViewFrameBuffer(ViewIdTonemap, invalidHandle);
}
#if CS_DEBUG_LUMAVG
bgfx::setViewRect(ViewIdDebug0, 0, 0, uint16_t(m_width), uint16_t(m_height));
bgfx::setViewRect(ViewIdDebug1, 306, 10, 512, 512);
bgfx::setViewRect(ViewIdDebug2, 823, 10, 512, 512);
bgfx::setViewRect(ViewIdDebug3, 306, 512+20, 512, 512);
bgfx::setViewRect(ViewIdDebug4, 823, 512+20, 512, 512);
bgfx::setViewRect(ViewIdDebug5, 1340, 512+20, 512, 512);
bgfx::setViewTransform(ViewIdDebug0, NULL, screenProj);
bgfx::setViewTransform(ViewIdDebug1, NULL, screenProj);
bgfx::setViewTransform(ViewIdDebug2, NULL, screenProj);
bgfx::setViewTransform(ViewIdDebug3, NULL, screenProj);
bgfx::setViewTransform(ViewIdDebug4, NULL, screenProj);
bgfx::setViewTransform(ViewIdDebug5, NULL, screenProj);
#endif //CS_DEBUG_LUMAVG
}
static void WriteFloatRGBA(std::array<float, 4> rgba,
std::byte *target,
_Interchange_buffer::pixel_layout layout,
_Interchange_buffer::alpha_mode alpha_mode)
{
// following calculations assume little-endian architecture.
switch (alpha_mode) {
case _Interchange_buffer::alpha_mode::ignore:
rgba[3] = 1.f;
break;
case _Interchange_buffer::alpha_mode::straight:
break;
case _Interchange_buffer::alpha_mode::premultiplied:
if( 1.f - rgba[3] > numeric_limits<float>::min() ) {
rgba[0] = rgba[0] * rgba[3];
rgba[1] = rgba[1] * rgba[3];
rgba[2] = rgba[2] * rgba[3];
}
break;
}
switch (layout) {
case _Interchange_buffer::pixel_layout::b8g8r8a8: {
auto p = (uint8_t *)target;
p[2] = uint8_t(rgba[0] * 255.f + 0.5f);
p[1] = uint8_t(rgba[1] * 255.f + 0.5f);
p[0] = uint8_t(rgba[2] * 255.f + 0.5f);
p[3] = uint8_t(rgba[3] * 255.f + 0.5f);
break;
}
case _Interchange_buffer::pixel_layout::a8r8g8b8: {
auto p = (uint8_t *)target;
p[1] = uint8_t(rgba[0] * 255.f + 0.5f);
p[2] = uint8_t(rgba[1] * 255.f + 0.5f);
p[3] = uint8_t(rgba[2] * 255.f + 0.5f);
p[0] = uint8_t(rgba[3] * 255.f + 0.5f);
break;
}
case _Interchange_buffer::pixel_layout::r8g8b8a8: {
auto p = (uint8_t *)target;
p[0] = uint8_t(rgba[0] * 255.f + 0.5f);
p[1] = uint8_t(rgba[1] * 255.f + 0.5f);
p[2] = uint8_t(rgba[2] * 255.f + 0.5f);
p[3] = uint8_t(rgba[3] * 255.f + 0.5f);
break;
}
case _Interchange_buffer::pixel_layout::a8b8g8r8: {
auto p = (uint8_t *)target;
p[3] = uint8_t(rgba[0] * 255.f + 0.5f);
p[2] = uint8_t(rgba[1] * 255.f + 0.5f);
p[1] = uint8_t(rgba[2] * 255.f + 0.5f);
p[0] = uint8_t(rgba[3] * 255.f + 0.5f);
break;
}
case _Interchange_buffer::pixel_layout::r5g6b5: {
auto &p = *(uint16_t *)target;
p = (uint16_t(rgba[0] * 31.f + 0.5f) << 0) |
(uint16_t(rgba[1] * 63.f + 0.5f) << 5) |
(uint16_t(rgba[2] * 31.f + 0.5f) << 11);
break;
}
case _Interchange_buffer::pixel_layout::b5g6r5: {
auto &p = *(uint16_t *)target;
p = (uint16_t(rgba[0] * 31.f + 0.5f) << 11)|
(uint16_t(rgba[1] * 63.f + 0.5f) << 5) |
(uint16_t(rgba[2] * 31.f + 0.5f) << 0) ;
break;
}
case _Interchange_buffer::pixel_layout::r5g5b5a1: {
auto &p = *(uint16_t *)target;
p = (uint16_t(rgba[0] * 31.f + 0.5f) << 0) |
(uint16_t(rgba[1] * 31.f + 0.5f) << 5) |
(uint16_t(rgba[2] * 31.f + 0.5f) << 10)|
(uint16_t(rgba[3] + 0.5f) << 15);
break;
}
case _Interchange_buffer::pixel_layout::b5g5r5a1: {
auto &p = *(uint16_t *)target;
p = (uint16_t(rgba[0] * 31.f + 0.5f) << 10)|
(uint16_t(rgba[1] * 31.f + 0.5f) << 5) |
(uint16_t(rgba[2] * 31.f + 0.5f) << 0) |
(uint16_t(rgba[3] + 0.5f) << 15);
break;
}
case _Interchange_buffer::pixel_layout::a1r5g5b5: {
auto &p = *(uint16_t *)target;
p = (uint16_t(rgba[0] * 31.f + 0.5f) << 1) |
(uint16_t(rgba[1] * 31.f + 0.5f) << 6) |
(uint16_t(rgba[2] * 31.f + 0.5f) << 11)|
(uint16_t(rgba[3] + 0.5f) << 0) ;
break;
}
case _Interchange_buffer::pixel_layout::a1b5g5r5: {
auto &p = *(uint16_t *)target;
p = (uint16_t(rgba[0] * 31.f + 0.5f) << 11)|
(uint16_t(rgba[1] * 31.f + 0.5f) << 6) |
(uint16_t(rgba[2] * 31.f + 0.5f) << 1) |
(uint16_t(rgba[3] + 0.5f) << 0) ;
break;
}
case _Interchange_buffer::pixel_layout::a8: {
auto &p = *(uint8_t *)target;
p = uint8_t(rgba[3] * 255.f + 0.5f);
break;
}
default: assert(0);
}
}
/* Check for automatic takeoff conditions being met using the following sequence:
* 1) Check for adequate GPS lock - if not return false
* 2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
* 3) Wait until the timer has reached the specified value (increments of 0.1 sec) and then check the GPS speed against the threshold
* 4) If the GPS speed is above the threshold and the attitude is within limits then return true and reset the timer
* 5) If the GPS speed and attitude within limits has not been achieved after 2.5 seconds, return false and reset the timer
* 6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
* NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
*/
bool Plane::auto_takeoff_check(void)
{
// this is a more advanced check that relies on TECS
uint32_t now = millis();
uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700);
// Reset states if process has been interrupted
if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timer interrupted AUTO");
takeoff_state.launchTimerStarted = false;
takeoff_state.last_tkoff_arm_time = 0;
takeoff_state.last_check_ms = now;
return false;
}
takeoff_state.last_check_ms = now;
// Check for bad GPS
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
// no auto takeoff without GPS lock
return false;
}
// Check for launch acceleration if set. NOTE: relies on TECS 50Hz processing
if (!is_zero(g.takeoff_throttle_min_accel) &&
SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
goto no_launch;
}
// we've reached the acceleration threshold, so start the timer
if (!takeoff_state.launchTimerStarted) {
takeoff_state.launchTimerStarted = true;
takeoff_state.last_tkoff_arm_time = now;
if (now - takeoff_state.last_report_ms > 2000) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec",
(double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f));
takeoff_state.last_report_ms = now;
}
}
// Only perform velocity check if not timed out
if ((now - takeoff_state.last_tkoff_arm_time) > wait_time_ms+100U) {
if (now - takeoff_state.last_report_ms > 2000) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timeout AUTO");
takeoff_state.last_report_ms = now;
}
goto no_launch;
}
// Check aircraft attitude for bad launch
if (ahrs.pitch_sensor <= -3000 ||
ahrs.pitch_sensor >= 4500 ||
(!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Bad launch AUTO");
goto no_launch;
}
// Check ground speed and time delay
if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) &&
((now - takeoff_state.last_tkoff_arm_time) >= wait_time_ms)) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed());
takeoff_state.launchTimerStarted = false;
takeoff_state.last_tkoff_arm_time = 0;
steer_state.locked_course_err = 0; // use current heading without any error offset
return true;
}
// we're not launching yet, but the timer is still going
return false;
no_launch:
takeoff_state.launchTimerStarted = false;
takeoff_state.last_tkoff_arm_time = 0;
return false;
}
const pm_char * eeRestoreModel(uint8_t i_fileDst, char *model_name)
{
char *buf = reusableBuffer.modelsel.mainname;
FIL restoreFile;
UINT read;
eeCheck(true);
if (!sdMounted()) {
return STR_NO_SDCARD;
}
strcpy(buf, STR_MODELS_PATH);
buf[sizeof(MODELS_PATH)-1] = '/';
strcpy(&buf[sizeof(MODELS_PATH)], model_name);
strcpy(&buf[strlen(buf)], STR_MODELS_EXT);
FRESULT result = f_open(&restoreFile, buf, FA_OPEN_EXISTING | FA_READ);
if (result != FR_OK) {
return SDCARD_ERROR(result);
}
if (f_size(&restoreFile) < 8) {
f_close(&restoreFile);
return STR_INCOMPATIBLE;
}
result = f_read(&restoreFile, (uint8_t *)buf, 8, &read);
if (result != FR_OK || read != 8) {
f_close(&restoreFile);
return SDCARD_ERROR(result);
}
uint8_t version = (uint8_t)buf[4];
if (*(uint32_t*)&buf[0] != O9X_FOURCC || version < FIRST_CONV_EEPROM_VER || version > EEPROM_VER || buf[5] != 'M') {
f_close(&restoreFile);
return STR_INCOMPATIBLE;
}
if (eeModelExists(i_fileDst)) {
eeDeleteModel(i_fileDst);
}
uint16_t size = min<uint16_t>(sizeof(g_model), *(uint16_t*)&buf[6]);
uint32_t address = eepromHeader.files[i_fileDst+1].zoneIndex * EEPROM_ZONE_SIZE;
// erase blocks
eepromEraseBlock(address);
eepromEraseBlock(address+EEPROM_BLOCK_SIZE);
// write header
EepromFileHeader header = { uint16_t(i_fileDst+1), size };
eepromWrite(address, (uint8_t *)&header, sizeof(header));
address += sizeof(header);
// write model
while (size > 0) {
uint16_t blockSize = min<uint16_t>(size, EEPROM_BUFFER_SIZE);
result = f_read(&restoreFile, eepromWriteBuffer, blockSize, &read);
if (result != FR_OK || read != blockSize) {
f_close(&g_oLogFile);
return SDCARD_ERROR(result);
}
eepromWrite(address, eepromWriteBuffer, blockSize);
size -= blockSize;
address += blockSize;
}
// write FAT
eepromHeader.files[i_fileDst+1].exists = 1;
eepromIncFatAddr();
eepromWriteState = EEPROM_WRITE_NEW_FAT;
eepromWriteWait();
eeLoadModelHeader(i_fileDst, &modelHeaders[i_fileDst]);
#if defined(PCBSKY9X)
if (version < EEPROM_VER) {
ConvertModel(i_fileDst, version);
loadModel(g_eeGeneral.currModel);
}
#endif
return NULL;
}
/*
For PPC (AIX & MAC), the first 8 integral and the first 13 f.p. parameters
arrive in a separate chunk of data that has been loaded from the registers.
The args pointer has been set to the start of the parameters BEYOND the ones
arriving in registers
*/
extern "C" nsresult ATTRIBUTE_USED
PrepareAndDispatch(nsXPTCStubBase* self, uint32_t methodIndex, uint32_t* args, uint32_t *gprData, double *fprData)
{
typedef struct {
uint32_t hi;
uint32_t lo; // have to move 64 bit entities as 32 bit halves since
} DU; // stack slots are not guaranteed 16 byte aligned
#define PARAM_BUFFER_COUNT 16
#define PARAM_GPR_COUNT 7
nsXPTCMiniVariant paramBuffer[PARAM_BUFFER_COUNT];
nsXPTCMiniVariant* dispatchParams = nullptr;
const nsXPTMethodInfo* info = nullptr;
uint8_t paramCount;
uint8_t i;
nsresult result = NS_ERROR_FAILURE;
NS_ASSERTION(self,"no self");
self->mEntry->GetMethodInfo(uint16_t(methodIndex), &info);
NS_ASSERTION(info,"no method info");
paramCount = info->GetParamCount();
// setup variant array pointer
if(paramCount > PARAM_BUFFER_COUNT)
dispatchParams = new nsXPTCMiniVariant[paramCount];
else
dispatchParams = paramBuffer;
NS_ASSERTION(dispatchParams,"no place for params");
uint32_t* ap = args;
uint32_t iCount = 0;
uint32_t fpCount = 0;
for(i = 0; i < paramCount; i++)
{
const nsXPTParamInfo& param = info->GetParam(i);
const nsXPTType& type = param.GetType();
nsXPTCMiniVariant* dp = &dispatchParams[i];
if(param.IsOut() || !type.IsArithmetic())
{
if (iCount < PARAM_GPR_COUNT)
dp->val.p = (void*) gprData[iCount++];
else
dp->val.p = (void*) *ap++;
continue;
}
// else
switch(type)
{
case nsXPTType::T_I8 : if (iCount < PARAM_GPR_COUNT)
dp->val.i8 = (int8_t) gprData[iCount++];
else
dp->val.i8 = (int8_t) *ap++;
break;
case nsXPTType::T_I16 : if (iCount < PARAM_GPR_COUNT)
dp->val.i16 = (int16_t) gprData[iCount++];
else
dp->val.i16 = (int16_t) *ap++;
break;
case nsXPTType::T_I32 : if (iCount < PARAM_GPR_COUNT)
dp->val.i32 = (int32_t) gprData[iCount++];
else
dp->val.i32 = (int32_t) *ap++;
break;
case nsXPTType::T_I64 : if (iCount < PARAM_GPR_COUNT)
((DU *)dp)->hi = (int32_t) gprData[iCount++];
else
((DU *)dp)->hi = (int32_t) *ap++;
if (iCount < PARAM_GPR_COUNT)
((DU *)dp)->lo = (uint32_t) gprData[iCount++];
else
((DU *)dp)->lo = (uint32_t) *ap++;
break;
case nsXPTType::T_U8 : if (iCount < PARAM_GPR_COUNT)
dp->val.u8 = (uint8_t) gprData[iCount++];
else
dp->val.u8 = (uint8_t) *ap++;
break;
case nsXPTType::T_U16 : if (iCount < PARAM_GPR_COUNT)
dp->val.u16 = (uint16_t) gprData[iCount++];
else
dp->val.u16 = (uint16_t) *ap++;
break;
case nsXPTType::T_U32 : if (iCount < PARAM_GPR_COUNT)
dp->val.u32 = (uint32_t) gprData[iCount++];
else
dp->val.u32 = (uint32_t) *ap++;
break;
case nsXPTType::T_U64 : if (iCount < PARAM_GPR_COUNT)
((DU *)dp)->hi = (uint32_t) gprData[iCount++];
else
((DU *)dp)->hi = (uint32_t) *ap++;
if (iCount < PARAM_GPR_COUNT)
((DU *)dp)->lo = (uint32_t) gprData[iCount++];
else
((DU *)dp)->lo = (uint32_t) *ap++;
break;
case nsXPTType::T_FLOAT : if (fpCount < 13) {
dp->val.f = (float) fprData[fpCount++];
if (iCount < PARAM_GPR_COUNT)
++iCount;
else
++ap;
}
else
dp->val.f = *((float*) ap++);
break;
case nsXPTType::T_DOUBLE : if (fpCount < 13) {
dp->val.d = (double) fprData[fpCount++];
if (iCount < PARAM_GPR_COUNT)
++iCount;
else
++ap;
if (iCount < PARAM_GPR_COUNT)
++iCount;
else
++ap;
}
else {
dp->val.f = *((double*) ap);
ap += 2;
}
break;
case nsXPTType::T_BOOL : if (iCount < PARAM_GPR_COUNT)
dp->val.b = (bool) gprData[iCount++];
else
dp->val.b = (bool) *ap++;
break;
case nsXPTType::T_CHAR : if (iCount < PARAM_GPR_COUNT)
dp->val.c = (char) gprData[iCount++];
else
dp->val.c = (char) *ap++;
break;
case nsXPTType::T_WCHAR : if (iCount < PARAM_GPR_COUNT)
dp->val.wc = (wchar_t) gprData[iCount++];
else
dp->val.wc = (wchar_t) *ap++;
break;
default:
NS_ERROR("bad type");
break;
}
}
result = self->mOuter->CallMethod((uint16_t)methodIndex,info,dispatchParams);
if(dispatchParams != paramBuffer)
delete [] dispatchParams;
return result;
}
byte tinyFAT::initFAT(byte speed)
{
mmc::initialize(speed);
// Read MBR
if (RES_OK == mmc::readSector(buffer, 0))
{
if ((buffer[0x01FE]==0x55) && (buffer[0x01FF]==0xAA))
{
MBR.part1Type=buffer[450];
MBR.part1Start = uint16_t(buffer[454])+(uint16_t(buffer[455])<<8)+(uint32_t(buffer[456])<<16)+(uint32_t(buffer[457])<<24);
MBR.part1Size = uint16_t(buffer[458])+(uint16_t(buffer[459])<<8)+(uint32_t(buffer[460])<<16)+(uint32_t(buffer[461])<<24);
}
else
{
return ERROR_MBR_SIGNATURE;
}
}
else
return ERROR_MBR_READ_ERROR;
if ((MBR.part1Type!=0x04) && (MBR.part1Type!=0x06) && (MBR.part1Type!=0x86))
{
return ERROR_MBR_INVALID_FS;
}
// Read Boot Sector
if (RES_OK == mmc::readSector(buffer, MBR.part1Start))
{
if ((buffer[0x01FE]==0x55) && (buffer[0x01FF]==0xAA))
{
BS.sectorsPerCluster = buffer[0x0D];
BS.reservedSectors = uint16_t(buffer[0x0E])+(uint16_t(buffer[0x0F])<<8);
BS.fatCopies = buffer[0x10];
BS.rootDirectoryEntries = uint16_t(buffer[0x11])+(uint16_t(buffer[0x12])<<8);
BS.totalFilesystemSectors = uint16_t(buffer[0x13])+(uint16_t(buffer[0x14])<<8);
if (BS.totalFilesystemSectors==0)
BS.totalFilesystemSectors = uint16_t(buffer[0x20])+(uint16_t(buffer[0x21])<<8)+(uint32_t(buffer[0x22])<<16)+(uint32_t(buffer[0x23])<<24);
BS.sectorsPerFAT = uint16_t(buffer[0x16])+(uint16_t(buffer[0x17])<<8);
BS.hiddenSectors = uint16_t(buffer[0x1C])+(uint16_t(buffer[0x1D])<<8)+(uint32_t(buffer[0x1E])<<16)+(uint32_t(buffer[0x1F])<<24);
BS.partitionSerialNum = uint16_t(buffer[0x27])+(uint16_t(buffer[0x28])<<8)+(uint32_t(buffer[0x29])<<16)+(uint32_t(buffer[0x2A])<<24);
firstDirSector = MBR.part1Start + BS.reservedSectors + (BS.fatCopies * BS.sectorsPerFAT);
BS.fat1Start = MBR.part1Start + BS.reservedSectors;
BS.fat2Start = BS.fat1Start + BS.sectorsPerFAT;
BS.partitionSize = float((MBR.part1Size*512)/float(1048576));
}
else
return ERROR_BOOTSEC_SIGNATURE;
}
else
return ERROR_BOOTSEC_READ_ERROR;
_inited=true;
return 0x00;
}
int16_t read_16(int32_t &address)
{
uint8_t l = read_8(address);
uint8_t h = read_8(address);
return ((uint16_t(h) & 0xff) << 8) | (uint16_t(l) & 0xff);
}
bool opcodeHasFlags(Opcode opcode, uint64_t flags) {
return OpInfo[uint16_t(opcode)].flags & flags;
}
bool RectanglePacker::addRectangle(uint16_t _width, uint16_t _height, uint16_t& _outX, uint16_t& _outY)
{
int best_height, best_index;
int32_t best_width;
Node* node;
Node* prev;
_outX = 0;
_outY = 0;
best_height = INT_MAX;
best_index = -1;
best_width = INT_MAX;
for (uint16_t ii = 0, num = uint16_t(m_skyline.size() ); ii < num; ++ii)
{
int32_t yy = fit(ii, _width, _height);
if (yy >= 0)
{
node = &m_skyline[ii];
if ( ( (yy + _height) < best_height)
|| ( ( (yy + _height) == best_height) && (node->width < best_width) ) )
{
best_height = uint16_t(yy) + _height;
best_index = ii;
best_width = node->width;
_outX = node->x;
_outY = uint16_t(yy);
}
}
}
if (best_index == -1)
{
return false;
}
Node newNode(_outX, _outY + _height, _width);
m_skyline.insert(m_skyline.begin() + best_index, newNode);
for (uint16_t ii = uint16_t(best_index + 1), num = uint16_t(m_skyline.size() ); ii < num; ++ii)
{
node = &m_skyline[ii];
prev = &m_skyline[ii - 1];
if (node->x < (prev->x + prev->width) )
{
uint16_t shrink = uint16_t(prev->x + prev->width - node->x);
node->x += shrink;
node->width -= shrink;
if (node->width <= 0)
{
m_skyline.erase(m_skyline.begin() + ii);
--ii;
--num;
}
else
{
break;
}
}
else
{
break;
}
}
merge();
m_usedSpace += _width * _height;
return true;
}
void ProxyRwCpp <SplFmt_INT16>::write_no_clip (const Ptr::Type &ptr, int src)
{
ptr [0] = uint16_t (src);
}
void XBeeResponse::getZBRxResponse(XBeeResponse &rxResponse) {
ZBRxResponse* zb = static_cast<ZBRxResponse*>(&rxResponse);
//TODO verify response api id matches this api for this response
// pass pointer array to subclass
zb->setFrameData(getFrameData());
setCommon(rxResponse);
zb->getRemoteAddress64().setMsb((uint32_t(getFrameData()[0]) << 24) + (uint32_t(getFrameData()[1]) << 16) + (uint16_t(getFrameData()[2]) << 8) + getFrameData()[3]);
zb->getRemoteAddress64().setLsb((uint32_t(getFrameData()[4]) << 24) + (uint32_t(getFrameData()[5]) << 16) + (uint16_t(getFrameData()[6]) << 8) + (getFrameData()[7]));
}
void ProxyRwCpp <SplFmt_INT16>::write_clip (const Ptr::Type &ptr, int src)
{
ptr [0] = uint16_t (fstb::limit (src, 0, (1 << C) - 1));
}
void lcd_menu_print_select()
{
if (!card.sdInserted)
{
LED_GLOW();
lcd_lib_encoder_pos = MAIN_MENU_ITEM_POS(0);
lcd_info_screen(lcd_menu_main);
lcd_lib_draw_string_centerP(15, PSTR("No SD-CARD!"));
lcd_lib_draw_string_centerP(25, PSTR("Please insert card"));
lcd_lib_update_screen();
card.release();
return;
}
if (!card.isOk())
{
lcd_info_screen(lcd_menu_main);
lcd_lib_draw_string_centerP(16, PSTR("Reading card..."));
lcd_lib_update_screen();
lcd_clear_cache();
card.initsd();
return;
}
if (LCD_CACHE_NR_OF_FILES() == 0xFF)
LCD_CACHE_NR_OF_FILES() = card.getnrfilenames();
if (card.errorCode())
{
LCD_CACHE_NR_OF_FILES() = 0xFF;
return;
}
uint8_t nrOfFiles = LCD_CACHE_NR_OF_FILES();
if (nrOfFiles == 0)
{
if (card.atRoot())
lcd_info_screen(lcd_menu_main, NULL, PSTR("OK"));
else
lcd_info_screen(lcd_menu_print_select, cardUpdir, PSTR("OK"));
lcd_lib_draw_string_centerP(25, PSTR("No files found!"));
lcd_lib_update_screen();
lcd_clear_cache();
return;
}
if (lcd_lib_button_pressed)
{
uint8_t selIndex = uint16_t(SELECTED_SCROLL_MENU_ITEM());
if (selIndex == 0)
{
if (card.atRoot())
{
lcd_change_to_menu(lcd_menu_main);
}else{
lcd_clear_cache();
lcd_lib_beep();
card.updir();
}
}else{
card.getfilename(selIndex - 1);
if (!card.filenameIsDir)
{
//Start print
active_extruder = 0;
card.openFile(card.filename, true);
if (card.isFileOpen() && !is_command_queued())
{
if (led_mode == LED_MODE_WHILE_PRINTING || led_mode == LED_MODE_BLINK_ON_DONE)
analogWrite(LED_PIN, 255 * int(led_brightness_level) / 100);
if (!card.longFilename[0])
strcpy(card.longFilename, card.filename);
card.longFilename[20] = '\0';
if (strchr(card.longFilename, '.')) strchr(card.longFilename, '.')[0] = '\0';
char buffer[64];
card.fgets(buffer, sizeof(buffer));
buffer[sizeof(buffer)-1] = '\0';
while (strlen(buffer) > 0 && buffer[strlen(buffer)-1] < ' ') buffer[strlen(buffer)-1] = '\0';
if (strcmp_P(buffer, PSTR(";FLAVOR:UltiGCode")) != 0)
{
card.fgets(buffer, sizeof(buffer));
buffer[sizeof(buffer)-1] = '\0';
while (strlen(buffer) > 0 && buffer[strlen(buffer)-1] < ' ') buffer[strlen(buffer)-1] = '\0';
}
card.setIndex(0);
if (strcmp_P(buffer, PSTR(";FLAVOR:UltiGCode")) == 0)
{
//New style GCode flavor without start/end code.
// Temperature settings, filament settings, fan settings, start and end-code are machine controlled.
target_temperature_bed = 0;
fanSpeedPercent = 0;
for(uint8_t e=0; e<EXTRUDERS; e++)
{
if (LCD_DETAIL_CACHE_MATERIAL(e) < 1)
continue;
target_temperature[e] = 0;//material[e].temperature;
target_temperature_bed = max(target_temperature_bed, material[e].bed_temperature);
fanSpeedPercent = max(fanSpeedPercent, material[0].fan_speed);
volume_to_filament_length[e] = 1.0 / (M_PI * (material[e].diameter / 2.0) * (material[e].diameter / 2.0));
extrudemultiply[e] = material[e].flow;
}
fanSpeed = 0;
enquecommand_P(PSTR("G28"));
enquecommand_P(PSTR("G1 F12000 X5 Y10"));
lcd_change_to_menu(lcd_menu_print_heatup);
}else{
//Classic gcode file
//Set the settings to defaults so the classic GCode has full control
fanSpeedPercent = 100;
for(uint8_t e=0; e<EXTRUDERS; e++)
{
volume_to_filament_length[e] = 1.0;
extrudemultiply[e] = 100;
}
lcd_change_to_menu(lcd_menu_print_classic_warning, MAIN_MENU_ITEM_POS(0));
}
}
}else{
lcd_lib_beep();
lcd_clear_cache();
card.chdir(card.filename);
SELECT_SCROLL_MENU_ITEM(0);
}
return;//Return so we do not continue after changing the directory or selecting a file. The nrOfFiles is invalid at this point.
}
}
lcd_scroll_menu(PSTR("SD CARD"), nrOfFiles+1, lcd_sd_menu_filename_callback, lcd_sd_menu_details_callback);
}
void AsyncUDPSocket::detachEventBase() {
DCHECK(eventBase_ && eventBase_->isInEventBaseThread());
registerHandler(uint16_t(NONE));
eventBase_ = nullptr;
EventHandler::detachEventBase();
}
extern "C" nsresult
PrepareAndDispatch(nsXPTCStubBase * self, uint32_t methodIndex,
uint64_t * args, uint64_t * gpregs, double *fpregs)
{
nsXPTCMiniVariant paramBuffer[PARAM_BUFFER_COUNT];
nsXPTCMiniVariant* dispatchParams = nullptr;
const nsXPTMethodInfo* info;
uint32_t paramCount;
uint32_t i;
nsresult result = NS_ERROR_FAILURE;
NS_ASSERTION(self,"no self");
self->mEntry->GetMethodInfo(uint16_t(methodIndex), &info);
NS_ASSERTION(info,"no method info");
if (!info)
return NS_ERROR_UNEXPECTED;
paramCount = info->GetParamCount();
// setup variant array pointer
if (paramCount > PARAM_BUFFER_COUNT)
dispatchParams = new nsXPTCMiniVariant[paramCount];
else
dispatchParams = paramBuffer;
NS_ASSERTION(dispatchParams,"no place for params");
if (!dispatchParams)
return NS_ERROR_OUT_OF_MEMORY;
uint64_t* ap = args;
uint32_t nr_gpr = 1; // skip one GPR register for 'that'
uint32_t nr_fpr = 0;
uint64_t value;
for (i = 0; i < paramCount; i++) {
const nsXPTParamInfo& param = info->GetParam(i);
const nsXPTType& type = param.GetType();
nsXPTCMiniVariant* dp = &dispatchParams[i];
if (!param.IsOut() && type == nsXPTType::T_DOUBLE) {
if (nr_fpr < FPR_COUNT)
dp->val.d = fpregs[nr_fpr++];
else
dp->val.d = *(double*) ap++;
continue;
}
else if (!param.IsOut() && type == nsXPTType::T_FLOAT) {
if (nr_fpr < FPR_COUNT)
// The value in %xmm register is already prepared to
// be retrieved as a float. Therefore, we pass the
// value verbatim, as a double without conversion.
dp->val.d = fpregs[nr_fpr++];
else
dp->val.f = *(float*) ap++;
continue;
}
else {
if (nr_gpr < GPR_COUNT)
value = gpregs[nr_gpr++];
else
value = *ap++;
}
if (param.IsOut() || !type.IsArithmetic()) {
dp->val.p = (void*) value;
continue;
}
switch (type) {
case nsXPTType::T_I8: dp->val.i8 = (int8_t) value; break;
case nsXPTType::T_I16: dp->val.i16 = (int16_t) value; break;
case nsXPTType::T_I32: dp->val.i32 = (int32_t) value; break;
case nsXPTType::T_I64: dp->val.i64 = (int64_t) value; break;
case nsXPTType::T_U8: dp->val.u8 = (uint8_t) value; break;
case nsXPTType::T_U16: dp->val.u16 = (uint16_t) value; break;
case nsXPTType::T_U32: dp->val.u32 = (uint32_t) value; break;
case nsXPTType::T_U64: dp->val.u64 = (uint64_t) value; break;
// Cast to uint8_t first, to remove garbage on upper 56 bits.
case nsXPTType::T_BOOL: dp->val.b = (bool)(uint8_t) value; break;
case nsXPTType::T_CHAR: dp->val.c = (char) value; break;
case nsXPTType::T_WCHAR: dp->val.wc = (wchar_t) value; break;
default:
NS_ERROR("bad type");
break;
}
}
result = self->mOuter->CallMethod((uint16_t) methodIndex, info, dispatchParams);
if (dispatchParams != paramBuffer)
delete [] dispatchParams;
return result;
}
extern "C" nsresult
PrepareAndDispatch(nsXPTCStubBase* self,
uint64_t methodIndex,
uint64_t* args,
uint64_t *gprData,
double *fprData)
{
nsXPTCMiniVariant paramBuffer[PARAM_BUFFER_COUNT];
nsXPTCMiniVariant* dispatchParams = NULL;
const nsXPTMethodInfo* info;
uint32_t paramCount;
uint32_t i;
nsresult result = NS_ERROR_FAILURE;
NS_ASSERTION(self,"no self");
self->mEntry->GetMethodInfo(uint16_t(methodIndex), &info);
NS_ASSERTION(info,"no method info");
if (! info)
return NS_ERROR_UNEXPECTED;
paramCount = info->GetParamCount();
// setup variant array pointer
if(paramCount > PARAM_BUFFER_COUNT)
dispatchParams = new nsXPTCMiniVariant[paramCount];
else
dispatchParams = paramBuffer;
NS_ASSERTION(dispatchParams,"no place for params");
if (! dispatchParams)
return NS_ERROR_OUT_OF_MEMORY;
uint64_t* ap = args;
uint64_t tempu64;
for(i = 0; i < paramCount; i++) {
const nsXPTParamInfo& param = info->GetParam(i);
const nsXPTType& type = param.GetType();
nsXPTCMiniVariant* dp = &dispatchParams[i];
if (!param.IsOut() && type == nsXPTType::T_DOUBLE) {
if (i < FPR_COUNT)
dp->val.d = fprData[i];
else
dp->val.d = *(double*) ap;
} else if (!param.IsOut() && type == nsXPTType::T_FLOAT) {
if (i < FPR_COUNT)
dp->val.f = (float) fprData[i]; // in registers floats are passed as doubles
else {
float *p = (float *)ap;
p++;
dp->val.f = *p;
}
} else { /* integer type or pointer */
if (i < GPR_COUNT)
tempu64 = gprData[i];
else
tempu64 = *ap;
if (param.IsOut() || !type.IsArithmetic())
dp->val.p = (void*) tempu64;
else if (type == nsXPTType::T_I8)
dp->val.i8 = (int8_t) tempu64;
else if (type == nsXPTType::T_I16)
dp->val.i16 = (int16_t) tempu64;
else if (type == nsXPTType::T_I32)
dp->val.i32 = (int32_t) tempu64;
else if (type == nsXPTType::T_I64)
dp->val.i64 = (int64_t) tempu64;
else if (type == nsXPTType::T_U8)
dp->val.u8 = (uint8_t) tempu64;
else if (type == nsXPTType::T_U16)
dp->val.u16 = (uint16_t) tempu64;
else if (type == nsXPTType::T_U32)
dp->val.u32 = (uint32_t) tempu64;
else if (type == nsXPTType::T_U64)
dp->val.u64 = (uint64_t) tempu64;
else if (type == nsXPTType::T_BOOL)
dp->val.b = (bool) tempu64;
else if (type == nsXPTType::T_CHAR)
dp->val.c = (char) tempu64;
else if (type == nsXPTType::T_WCHAR)
dp->val.wc = (wchar_t) tempu64;
else
NS_ERROR("bad type");
}
if (i >= 7)
ap++;
}
result = self->mOuter->CallMethod((uint16_t) methodIndex, info,
dispatchParams);
if (dispatchParams != paramBuffer)
delete [] dispatchParams;
return result;
}
status_t AudioTrack::createTrack(
int streamType,
uint32_t sampleRate,
int format,
int channelCount,
int frameCount,
uint32_t flags,
const sp<IMemory>& sharedBuffer,
audio_io_handle_t output,
bool enforceFrameCount)
{
status_t status;
const sp<IAudioFlinger>& audioFlinger = AudioSystem::get_audio_flinger();
if (audioFlinger == 0) {
LOGE("Could not get audioflinger");
return NO_INIT;
}
int afSampleRate;
if (AudioSystem::getOutputSamplingRate(&afSampleRate, streamType) != NO_ERROR) {
return NO_INIT;
}
int afFrameCount;
if (AudioSystem::getOutputFrameCount(&afFrameCount, streamType) != NO_ERROR) {
return NO_INIT;
}
uint32_t afLatency;
if (AudioSystem::getOutputLatency(&afLatency, streamType) != NO_ERROR) {
return NO_INIT;
}
mNotificationFramesAct = mNotificationFramesReq;
if (!AudioSystem::isLinearPCM(format)) {
if (sharedBuffer != 0) {
frameCount = sharedBuffer->size();
}
} else {
// Ensure that buffer depth covers at least audio hardware latency
uint32_t minBufCount = afLatency / ((1000 * afFrameCount)/afSampleRate);
if (minBufCount < 2) minBufCount = 2;
int minFrameCount = (afFrameCount*sampleRate*minBufCount)/afSampleRate;
if (sharedBuffer == 0) {
if (frameCount == 0) {
frameCount = minFrameCount;
}
if (mNotificationFramesAct == 0) {
mNotificationFramesAct = frameCount/2;
}
// Make sure that application is notified with sufficient margin
// before underrun
if (mNotificationFramesAct > (uint32_t)frameCount/2) {
mNotificationFramesAct = frameCount/2;
}
if (frameCount < minFrameCount) {
if (enforceFrameCount) {
LOGE("Invalid buffer size: minFrameCount %d, frameCount %d", minFrameCount, frameCount);
return BAD_VALUE;
} else {
frameCount = minFrameCount;
}
}
} else {
// Ensure that buffer alignment matches channelcount
if (((uint32_t)sharedBuffer->pointer() & (channelCount | 1)) != 0) {
LOGE("Invalid buffer alignement: address %p, channelCount %d", sharedBuffer->pointer(), channelCount);
return BAD_VALUE;
}
frameCount = sharedBuffer->size()/channelCount/sizeof(int16_t);
}
}
LOGD("Request AudioFlinger to create track");
sp<IAudioTrack> track = audioFlinger->createTrack(getpid(),
streamType,
sampleRate,
format,
channelCount,
frameCount,
((uint16_t)flags) << 16,
sharedBuffer,
output,
&mSessionId,
&status);
if (track == 0) {
LOGE("AudioFlinger could not create track, status: %d", status);
return status;
}
sp<IMemory> cblk = track->getCblk();
if (cblk == 0) {
LOGE("Could not get control block");
return NO_INIT;
}
mAudioTrack.clear();
mAudioTrack = track;
mCblkMemory.clear();
mCblkMemory = cblk;
mCblk = static_cast<audio_track_cblk_t*>(cblk->pointer());
mCblk->flags |= CBLK_DIRECTION_OUT;
if (sharedBuffer == 0) {
mCblk->buffers = (char*)mCblk + sizeof(audio_track_cblk_t);
} else {
mCblk->buffers = sharedBuffer->pointer();
// Force buffer full condition as data is already present in shared memory
mCblk->stepUser(mCblk->frameCount);
}
mCblk->volumeLR = (uint32_t(uint16_t(mVolume[RIGHT] * 0x1000)) << 16) | uint16_t(mVolume[LEFT] * 0x1000);
mCblk->sendLevel = uint16_t(mSendLevel * 0x1000);
mAudioTrack->attachAuxEffect(mAuxEffectId);
mCblk->bufferTimeoutMs = MAX_STARTUP_TIMEOUT_MS;
mCblk->waitTimeMs = 0;
mRemainingFrames = mNotificationFramesAct;
mLatency = afLatency + (1000*mCblk->frameCount) / sampleRate;
return NO_ERROR;
}
// Returns true if a node was read successfully, false on EOF
bool XMLReader::ReadInternal()
{
switch (mNodeType) {
case kNone:
FillInputBuffer();
if ((mInputEnd - mInputStart) >= 2) {
uint16_t x = (uint16_t(mInputBuffer[0]) << 8) | mInputBuffer[1];
switch (x) {
case 0xFEFF:
case 0x003C:
mConverter.Reset(TextEncoding::UTF16BE());
break;
case 0xFFFE:
case 0x3C00:
mConverter.Reset(TextEncoding::UTF16LE());
break;
}
}
mNodeType = kDocument;
return true;
case kDocument:
// An XML document can start with:
// document ::= prolog element Misc*
// prolog ::= XMLDecl? Misc* (doctypedecl Misc*)?
// XMLDecl ::= '<?xml' VersionInfo EncodingDecl? SDDecl? S? '?>'
// Misc ::= Comment | PI | S
// doctypedecl ::= '<!DOCTYPE' S Name (S ExternalID)? S? ('[' intSubset ']' S?)? '>'
// Comment ::= '<!--' ((Char - '-') | ('-' (Char - '-')))* '-->'
// PI ::= '<?' PITarget (S (Char* - (Char* '?>' Char*)))? '?>'
// S ::= (#x20 | #x9 | #xD | #xA)+
// If the XML file starts with a byte order mark, throw it away.
// The earlier code for the kNone case has already used it to
// set the default encoding.
ParseChar(UnicodeChar(0xFEFF));
if (BufferStartsWith("<?xml")) {
if (! ParseXmlDeclaration()) return false;
UnicodeString encodingName = GetAttribute("encoding");
if (encodingName.empty()) return true;
TextEncoding newEncoding = TextEncoding::WebCharset(encodingName.c_str());
if (newEncoding == mConverter.GetSourceEncoding()) return true;
// The encoding in the XML declaration is different from the one
// we assumed, so we have to reset all the input buffering and
// re-parse the XmlDeclaration.
mConverter.Reset(newEncoding);
mInput.Restart();
mInputStart = mInputEnd = mInputBuffer;
mOutputStart = mOutputEnd = mOutputBuffer;
ParseChar(UnicodeChar(0xFEFF));
return ParseXmlDeclaration();
}
else if (StartsWithWhitespace()) return ParseRequiredWhitespace();
//else if (BufferStartsWith("<!--")) return ParseComment();
//else if (BufferStartsWith("<?")) return ParseProcessingInstruction();
//else if (BufferStartsWith("<!DOCTYPE")) return ParseDocumentType();
else if (BufferStartsWith("<")) return ParseElement();
else return false;
case kXmlDeclaration:
case kElement:
case kEndElement:
case kText:
case kWhitespace:
if (BufferStartsWith("</")) return ParseEndElement();
else if (BufferStartsWith("<")) return ParseElement();
else return ParseText();
}
return false;
}
void WebMBufferedParser::Append(const unsigned char* aBuffer, uint32_t aLength,
nsTArray<WebMTimeDataOffset>& aMapping,
ReentrantMonitor& aReentrantMonitor)
{
static const uint32_t EBML_ID = 0x1a45dfa3;
static const uint32_t SEGMENT_ID = 0x18538067;
static const uint32_t SEGINFO_ID = 0x1549a966;
static const uint32_t TRACKS_ID = 0x1654AE6B;
static const uint32_t CLUSTER_ID = 0x1f43b675;
static const uint32_t TIMECODESCALE_ID = 0x2ad7b1;
static const unsigned char TIMECODE_ID = 0xe7;
static const unsigned char BLOCK_ID = 0xa1;
static const unsigned char SIMPLEBLOCK_ID = 0xa3;
static const uint32_t BLOCK_TIMECODE_LENGTH = 2;
static const unsigned char CLUSTER_SYNC_ID[] = { 0x1f, 0x43, 0xb6, 0x75 };
const unsigned char* p = aBuffer;
// Parse each byte in aBuffer one-by-one, producing timecodes and updating
// aMapping as we go. Parser pauses at end of stream (which may be at any
// point within the parse) and resumes parsing the next time Append is
// called with new data.
while (p < aBuffer + aLength) {
switch (mState) {
case READ_ELEMENT_ID:
mVIntRaw = true;
mState = READ_VINT;
mNextState = READ_ELEMENT_SIZE;
break;
case READ_ELEMENT_SIZE:
mVIntRaw = false;
mElement.mID = mVInt;
mState = READ_VINT;
mNextState = PARSE_ELEMENT;
break;
case FIND_CLUSTER_SYNC:
if (*p++ == CLUSTER_SYNC_ID[mClusterSyncPos]) {
mClusterSyncPos += 1;
} else {
mClusterSyncPos = 0;
}
if (mClusterSyncPos == sizeof(CLUSTER_SYNC_ID)) {
mVInt.mValue = CLUSTER_ID;
mVInt.mLength = sizeof(CLUSTER_SYNC_ID);
mState = READ_ELEMENT_SIZE;
}
break;
case PARSE_ELEMENT:
mElement.mSize = mVInt;
switch (mElement.mID.mValue) {
case SEGMENT_ID:
mState = READ_ELEMENT_ID;
break;
case SEGINFO_ID:
mGotTimecodeScale = true;
mState = READ_ELEMENT_ID;
break;
case TIMECODE_ID:
mVInt = VInt();
mVIntLeft = mElement.mSize.mValue;
mState = READ_VINT_REST;
mNextState = READ_CLUSTER_TIMECODE;
break;
case TIMECODESCALE_ID:
mVInt = VInt();
mVIntLeft = mElement.mSize.mValue;
mState = READ_VINT_REST;
mNextState = READ_TIMECODESCALE;
break;
case CLUSTER_ID:
mClusterOffset = mCurrentOffset + (p - aBuffer) -
(mElement.mID.mLength + mElement.mSize.mLength);
// Handle "unknown" length;
if (mElement.mSize.mValue + 1 != uint64_t(1) << (mElement.mSize.mLength * 7)) {
mClusterEndOffset = mClusterOffset + mElement.mID.mLength + mElement.mSize.mLength + mElement.mSize.mValue;
} else {
mClusterEndOffset = -1;
}
mState = READ_ELEMENT_ID;
break;
case SIMPLEBLOCK_ID:
/* FALLTHROUGH */
case BLOCK_ID:
mBlockSize = mElement.mSize.mValue;
mBlockTimecode = 0;
mBlockTimecodeLength = BLOCK_TIMECODE_LENGTH;
mBlockOffset = mCurrentOffset + (p - aBuffer) -
(mElement.mID.mLength + mElement.mSize.mLength);
mState = READ_VINT;
mNextState = READ_BLOCK_TIMECODE;
break;
case TRACKS_ID:
mSkipBytes = mElement.mSize.mValue;
mState = CHECK_INIT_FOUND;
break;
case EBML_ID:
mLastInitStartOffset = mCurrentOffset + (p - aBuffer) -
(mElement.mID.mLength + mElement.mSize.mLength);
/* FALLTHROUGH */
default:
mSkipBytes = mElement.mSize.mValue;
mState = SKIP_DATA;
mNextState = READ_ELEMENT_ID;
break;
}
break;
case READ_VINT: {
unsigned char c = *p++;
uint32_t mask;
mVInt.mLength = VIntLength(c, &mask);
mVIntLeft = mVInt.mLength - 1;
mVInt.mValue = mVIntRaw ? c : c & ~mask;
mState = READ_VINT_REST;
break;
}
case READ_VINT_REST:
if (mVIntLeft) {
mVInt.mValue <<= 8;
mVInt.mValue |= *p++;
mVIntLeft -= 1;
} else {
mState = mNextState;
}
break;
case READ_TIMECODESCALE:
MOZ_ASSERT(mGotTimecodeScale);
mTimecodeScale = mVInt.mValue;
mState = READ_ELEMENT_ID;
break;
case READ_CLUSTER_TIMECODE:
mClusterTimecode = mVInt.mValue;
mState = READ_ELEMENT_ID;
break;
case READ_BLOCK_TIMECODE:
if (mBlockTimecodeLength) {
mBlockTimecode <<= 8;
mBlockTimecode |= *p++;
mBlockTimecodeLength -= 1;
} else {
// It's possible we've parsed this data before, so avoid inserting
// duplicate WebMTimeDataOffset entries.
{
ReentrantMonitorAutoEnter mon(aReentrantMonitor);
int64_t endOffset = mBlockOffset + mBlockSize +
mElement.mID.mLength + mElement.mSize.mLength;
uint32_t idx = aMapping.IndexOfFirstElementGt(endOffset);
if (idx == 0 || aMapping[idx - 1] != endOffset) {
// Don't insert invalid negative timecodes.
if (mBlockTimecode >= 0 || mClusterTimecode >= uint16_t(abs(mBlockTimecode))) {
MOZ_ASSERT(mGotTimecodeScale);
uint64_t absTimecode = mClusterTimecode + mBlockTimecode;
absTimecode *= mTimecodeScale;
WebMTimeDataOffset entry(endOffset, absTimecode, mLastInitStartOffset,
mClusterOffset, mClusterEndOffset);
aMapping.InsertElementAt(idx, entry);
}
}
}
// Skip rest of block header and the block's payload.
mBlockSize -= mVInt.mLength;
mBlockSize -= BLOCK_TIMECODE_LENGTH;
mSkipBytes = uint32_t(mBlockSize);
mState = SKIP_DATA;
mNextState = READ_ELEMENT_ID;
}
break;
case SKIP_DATA:
if (mSkipBytes) {
uint32_t left = aLength - (p - aBuffer);
left = std::min(left, mSkipBytes);
p += left;
mSkipBytes -= left;
}
if (!mSkipBytes) {
mBlockEndOffset = mCurrentOffset + (p - aBuffer);
mState = mNextState;
}
break;
case CHECK_INIT_FOUND:
if (mSkipBytes) {
uint32_t left = aLength - (p - aBuffer);
left = std::min(left, mSkipBytes);
p += left;
mSkipBytes -= left;
}
if (!mSkipBytes) {
if (mInitEndOffset < 0) {
mInitEndOffset = mCurrentOffset + (p - aBuffer);
mBlockEndOffset = mCurrentOffset + (p - aBuffer);
}
mState = READ_ELEMENT_ID;
}
break;
}
}
NS_ASSERTION(p == aBuffer + aLength, "Must have parsed to end of data.");
mCurrentOffset += aLength;
}
nsresult
nsSocketTransportService::DoPollIteration(TimeDuration *pollDuration)
{
SOCKET_LOG(("STS poll iter\n"));
int32_t i, count;
//
// poll loop
//
// walk active list backwards to see if any sockets should actually be
// idle, then walk the idle list backwards to see if any idle sockets
// should become active. take care to check only idle sockets that
// were idle to begin with ;-)
//
count = mIdleCount;
for (i=mActiveCount-1; i>=0; --i) {
//---
SOCKET_LOG((" active [%u] { handler=%p condition=%x pollflags=%hu }\n", i,
mActiveList[i].mHandler,
mActiveList[i].mHandler->mCondition,
mActiveList[i].mHandler->mPollFlags));
//---
if (NS_FAILED(mActiveList[i].mHandler->mCondition))
DetachSocket(mActiveList, &mActiveList[i]);
else {
uint16_t in_flags = mActiveList[i].mHandler->mPollFlags;
if (in_flags == 0)
MoveToIdleList(&mActiveList[i]);
else {
// update poll flags
mPollList[i+1].in_flags = in_flags;
mPollList[i+1].out_flags = 0;
}
}
}
for (i=count-1; i>=0; --i) {
//---
SOCKET_LOG((" idle [%u] { handler=%p condition=%x pollflags=%hu }\n", i,
mIdleList[i].mHandler,
mIdleList[i].mHandler->mCondition,
mIdleList[i].mHandler->mPollFlags));
//---
if (NS_FAILED(mIdleList[i].mHandler->mCondition))
DetachSocket(mIdleList, &mIdleList[i]);
else if (mIdleList[i].mHandler->mPollFlags != 0)
MoveToPollList(&mIdleList[i]);
}
SOCKET_LOG((" calling PR_Poll [active=%u idle=%u]\n", mActiveCount, mIdleCount));
#if defined(XP_WIN)
// 30 active connections is the historic limit before firefox 7's 256. A few
// windows systems have troubles with the higher limit, so actively probe a
// limit the first time we exceed 30.
if ((mActiveCount > 30) && !mProbedMaxCount)
ProbeMaxCount();
#endif
// Measures seconds spent while blocked on PR_Poll
uint32_t pollInterval = 0;
int32_t n = 0;
*pollDuration = 0;
if (!gIOService->IsNetTearingDown()) {
// Let's not do polling during shutdown.
n = Poll(&pollInterval, pollDuration);
}
if (n < 0) {
SOCKET_LOG((" PR_Poll error [%d] os error [%d]\n", PR_GetError(),
PR_GetOSError()));
}
else {
//
// service "active" sockets...
//
uint32_t numberOfOnSocketReadyCalls = 0;
for (i=0; i<int32_t(mActiveCount); ++i) {
PRPollDesc &desc = mPollList[i+1];
SocketContext &s = mActiveList[i];
if (n > 0 && desc.out_flags != 0) {
s.mElapsedTime = 0;
s.mHandler->OnSocketReady(desc.fd, desc.out_flags);
numberOfOnSocketReadyCalls++;
}
// check for timeout errors unless disabled...
else if (s.mHandler->mPollTimeout != UINT16_MAX) {
// update elapsed time counter
// (NOTE: We explicitly cast UINT16_MAX to be an unsigned value
// here -- otherwise, some compilers will treat it as signed,
// which makes them fire signed/unsigned-comparison build
// warnings for the comparison against 'pollInterval'.)
if (MOZ_UNLIKELY(pollInterval >
static_cast<uint32_t>(UINT16_MAX) -
s.mElapsedTime))
s.mElapsedTime = UINT16_MAX;
else
s.mElapsedTime += uint16_t(pollInterval);
// check for timeout expiration
if (s.mElapsedTime >= s.mHandler->mPollTimeout) {
s.mElapsedTime = 0;
s.mHandler->OnSocketReady(desc.fd, -1);
numberOfOnSocketReadyCalls++;
}
}
}
if (mTelemetryEnabledPref) {
Telemetry::Accumulate(
Telemetry::STS_NUMBER_OF_ONSOCKETREADY_CALLS,
numberOfOnSocketReadyCalls);
}
//
// check for "dead" sockets and remove them (need to do this in
// reverse order obviously).
//
for (i=mActiveCount-1; i>=0; --i) {
if (NS_FAILED(mActiveList[i].mHandler->mCondition))
DetachSocket(mActiveList, &mActiveList[i]);
}
if (n != 0 && (mPollList[0].out_flags & (PR_POLL_READ | PR_POLL_EXCEPT))) {
DebugMutexAutoLock lock(mLock);
// acknowledge pollable event (should not block)
if (mPollableEvent &&
((mPollList[0].out_flags & PR_POLL_EXCEPT) ||
!mPollableEvent->Clear())) {
// On Windows, the TCP loopback connection in the
// pollable event may become broken when a laptop
// switches between wired and wireless networks or
// wakes up from hibernation. We try to create a
// new pollable event. If that fails, we fall back
// on "busy wait".
NS_WARNING("Trying to repair mPollableEvent");
mPollableEvent.reset(new PollableEvent());
if (!mPollableEvent->Valid()) {
mPollableEvent = nullptr;
}
SOCKET_LOG(("running socket transport thread without "
"a pollable event now valid=%d", mPollableEvent->Valid()));
mPollList[0].fd = mPollableEvent ? mPollableEvent->PollableFD() : nullptr;
mPollList[0].in_flags = PR_POLL_READ | PR_POLL_EXCEPT;
mPollList[0].out_flags = 0;
}
}
}
return NS_OK;
}
void depth_to_laser::depth2laser(const sensor_msgs::ImageConstPtr& depth_msg, sensor_msgs::LaserScanPtr scan_msg, const Params param)
{
// Calculate vars
double unit_scaling = depthimage_to_laserscan::DepthTraits<uint16_t>::toMeters(uint16_t(1));
float constant_x = unit_scaling / param._f;
uint32_t ranges_size = depth_msg->width;
double kinect_angle_max = -atan2((double)(0 - param._c_u) * constant_x, unit_scaling);
double kinect_angle_min = -atan2((double)(depth_msg->width-1 - param._c_u) * constant_x, unit_scaling);
double kinect_angle_increment = (kinect_angle_max - kinect_angle_min) / (depth_msg->width - 1);
// Fill the kinect fixed message
sensor_msgs::LaserScanPtr kinect_scan_msg(new sensor_msgs::LaserScan());
kinect_scan_msg->header = depth_msg->header;
kinect_scan_msg->header.frame_id = "kinect2";
kinect_scan_msg->time_increment = 0.0;
kinect_scan_msg->scan_time = 0.033;
kinect_scan_msg->range_min = 0.45;
kinect_scan_msg->range_max = 5.0;
kinect_scan_msg->angle_min = kinect_angle_min;
kinect_scan_msg->angle_max = kinect_angle_max;
kinect_scan_msg->angle_increment = kinect_angle_increment;
kinect_scan_msg->ranges.resize(ranges_size);
kinect_scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());
kinect_scan_msg->intensities.assign(ranges_size, 0.5);
const uint16_t* depth_row = reinterpret_cast<const uint16_t*>(&depth_msg->data[0]);
int row_step = depth_msg->step / sizeof(uint16_t);
int offset = (int)(param._c_v-param._scan_height/2);
depth_row += offset*row_step;
for(int v = offset; v < offset+param._scan_height; v++, depth_row += row_step)
{
for (int u = 0; u < (int)depth_msg->width; u++) // Loop in row
{
uint16_t depth = depth_row[u];
double kinect_r = depth; // Assign to pass through NaNs and Infs
double kinect_th = -atan2((double)(u - param._c_u) * constant_x, unit_scaling); // -atan2(x, z)
int kinect_index = (kinect_th - kinect_scan_msg->angle_min) / kinect_scan_msg->angle_increment;
if (depthimage_to_laserscan::DepthTraits<uint16_t>::valid(depth)) // Not NaN or Inf
{
// Calculate in XYZ
double kinect_x = (u - param._c_u) * depth * constant_x;
double kinect_z = depthimage_to_laserscan::DepthTraits<uint16_t>::toMeters(depth);
// Calculate actual distance
kinect_r = sqrt(pow(kinect_x, 2.0) + pow(kinect_z, 2.0));
}
// Determine if this point should be used.
if (use_point(kinect_r, kinect_scan_msg->ranges[kinect_index], kinect_scan_msg->range_min, kinect_scan_msg->range_max))
{
kinect_scan_msg->ranges[kinect_index] = kinect_r;
kinect_scan_msg->intensities[kinect_index] = 0.5;
}
}
}
// Fill the robot fixed message
sensor_msgs::LaserScanPtr robot_scan_msg(new sensor_msgs::LaserScan());
robot_scan_msg->header = depth_msg->header;
robot_scan_msg->header.frame_id = "robot";
robot_scan_msg->time_increment = 0.0;
robot_scan_msg->scan_time = 0.033;
robot_scan_msg->range_min = 0.45;
robot_scan_msg->range_max = 5.0;
double robot_min_x = -robot_scan_msg->range_max * sin(kinect_scan_msg->angle_min) + param._kinect_to_laser_x + param._laser_to_robot_x;
double robot_min_z = robot_scan_msg->range_max * cos(kinect_scan_msg->angle_min) + param._kinect_to_laser_z + param._laser_to_robot_z;
double robot_max_x = -robot_scan_msg->range_max * sin(kinect_scan_msg->angle_max) + param._kinect_to_laser_x + param._laser_to_robot_x;
double robot_max_z = robot_scan_msg->range_max * cos(kinect_scan_msg->angle_max) + param._kinect_to_laser_z + param._laser_to_robot_z;
double robot_angle_min = -atan2(robot_min_x, robot_min_z) + param._kinect_to_laser_theta;
double robot_angle_max = -atan2(robot_max_x, robot_max_z) + param._kinect_to_laser_theta;
double robot_angle_increment = (robot_angle_max - robot_angle_min) / (depth_msg->width - 1);
robot_scan_msg->angle_min = robot_angle_min;
robot_scan_msg->angle_max = robot_angle_max;
robot_scan_msg->angle_increment = robot_angle_increment;
robot_scan_msg->ranges.resize(ranges_size);
robot_scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());
robot_scan_msg->intensities.assign(ranges_size, 0.5);
for (int i = 0; i < ranges_size; i++)
{
double kinect_r = kinect_scan_msg->ranges[i];
double kinect_th = kinect_angle_min + i * kinect_angle_increment;
// New point (In the robot coordinate)
double robot_x = -kinect_r * sin(kinect_th + param._kinect_to_laser_theta) + param._kinect_to_laser_x + param._laser_to_robot_x;
double robot_z = kinect_r * cos(kinect_th + param._kinect_to_laser_theta) + param._kinect_to_laser_z + param._laser_to_robot_z;
// New angle (In the robot coordinate)
double robot_th = -atan2(robot_x, robot_z);
if (!std::isfinite(robot_th) || isnan(robot_th) || robot_th < robot_scan_msg->angle_min || robot_th > robot_scan_msg->angle_max) continue;
int robot_index = (robot_th - robot_scan_msg->angle_min) / robot_scan_msg->angle_increment;
double robot_r = sqrt(pow(robot_x, 2.0) + pow(robot_z, 2.0));
// Determine if this point should be used.
if (use_point(robot_r, robot_scan_msg->ranges[robot_index], robot_scan_msg->range_min, robot_scan_msg->range_max))
{
robot_scan_msg->ranges[robot_index] = robot_r;
robot_scan_msg->intensities[robot_index] = 0.5;
}
}
/******** kinect_scan_msg[frame_id] = "kinect2" | robot_scan_msg[frame_id] = "bankrobot" ********/
//*scan_msg = *kinect_scan_msg;
*scan_msg = *robot_scan_msg;
}
// special write function for strings
void write_pod(std::string &buffer, const std::string &value) {
uint16_t len = uint16_t(value.size()); write_pod(buffer, len);
buffer.append(value);
}
extern "C" nsresult ATTRIBUTE_USED
PrepareAndDispatch(nsXPTCStubBase* self, uint64_t methodIndex, uint64_t* args)
{
#define PARAM_BUFFER_COUNT 16
nsXPTCMiniVariant paramBuffer[PARAM_BUFFER_COUNT];
nsXPTCMiniVariant* dispatchParams = nullptr;
const nsXPTMethodInfo* info;
uint8_t paramCount;
uint8_t i;
nsresult result = NS_ERROR_FAILURE;
NS_ASSERTION(self,"no self");
self->mEntry->GetMethodInfo(uint16_t(methodIndex), &info);
NS_ASSERTION(info,"no interface info");
paramCount = info->GetParamCount();
// setup variant array pointer
if(paramCount > PARAM_BUFFER_COUNT)
dispatchParams = new nsXPTCMiniVariant[paramCount];
else
dispatchParams = paramBuffer;
NS_ASSERTION(dispatchParams,"no place for params");
uint64_t* ap = args;
for(i = 0; i < paramCount; i++, ap++)
{
const nsXPTParamInfo& param = info->GetParam(i);
const nsXPTType& type = param.GetType();
nsXPTCMiniVariant* dp = &dispatchParams[i];
if(param.IsOut() || !type.IsArithmetic())
{
dp->val.p = (void*) *ap;
continue;
}
// else
switch(type)
{
case nsXPTType::T_I8 : dp->val.i8 = *((int64_t*) ap); break;
case nsXPTType::T_I16 : dp->val.i16 = *((int64_t*) ap); break;
case nsXPTType::T_I32 : dp->val.i32 = *((int64_t*) ap); break;
case nsXPTType::T_DOUBLE : dp->val.d = *((double*) ap); break;
case nsXPTType::T_U64 : dp->val.u64 = *((uint64_t*)ap); break;
case nsXPTType::T_I64 : dp->val.i64 = *((int64_t*) ap); break;
case nsXPTType::T_U8 : dp->val.u8 = *((uint64_t*)ap); break;
case nsXPTType::T_U16 : dp->val.u16 = *((uint64_t*)ap); break;
case nsXPTType::T_U32 : dp->val.u32 = *((uint64_t*)ap); break;
case nsXPTType::T_FLOAT : dp->val.f = ((float*) ap)[1]; break;
case nsXPTType::T_BOOL : dp->val.b = *((uint64_t*)ap); break;
case nsXPTType::T_CHAR : dp->val.c = *((uint64_t*)ap); break;
case nsXPTType::T_WCHAR : dp->val.wc = *((int64_t*) ap); break;
default:
NS_ERROR("bad type");
break;
}
}
result = self->mOuter->CallMethod((uint16_t)methodIndex, info, dispatchParams);
if(dispatchParams != paramBuffer)
delete [] dispatchParams;
return result;
}
TED7360::TED7360() : M7501()
{
tedRegisters[0x07] = uint8_t(0x00); // default to PAL mode
for (int i = 0; i < int(sizeof(callbacks) / sizeof(TEDCallback)); i++) {
callbacks[i].func = (void (*)(void *)) 0;
callbacks[i].userData = (void *) 0;
callbacks[i].nxt0 = (TEDCallback *) 0;
callbacks[i].nxt1 = (TEDCallback *) 0;
}
firstCallback0 = (TEDCallback *) 0;
firstCallback1 = (TEDCallback *) 0;
// create initial memory map
ramSegments = 0;
ramPatternCode = 0UL;
randomSeed = 0;
Plus4Emu::setRandomSeed(randomSeed,
Plus4Emu::Timer::getRandomSeedFromTime());
for (int i = 0; i < 256; i++)
segmentTable[i] = (uint8_t *) 0;
try {
setRAMSize(64);
}
catch (...) {
for (int i = 0; i < 256; i++) {
if (segmentTable[i] != (uint8_t *) 0) {
delete[] segmentTable[i];
segmentTable[i] = (uint8_t *) 0;
}
}
throw;
}
setMemoryCallbackUserData(this);
for (uint16_t i = 0x0000; i <= 0x0FFF; i++) {
setMemoryReadCallback(i, &read_memory_0000_to_0FFF);
setMemoryWriteCallback(i, &write_memory_0000_to_0FFF);
}
for (uint16_t i = 0x1000; i <= 0x3FFF; i++) {
setMemoryReadCallback(i, &read_memory_1000_to_3FFF);
setMemoryWriteCallback(i, &write_memory_1000_to_3FFF);
}
for (uint16_t i = 0x4000; i <= 0x7FFF; i++) {
setMemoryReadCallback(i, &read_memory_4000_to_7FFF);
setMemoryWriteCallback(i, &write_memory_4000_to_7FFF);
}
for (uint16_t i = 0x8000; i <= 0xBFFF; i++) {
setMemoryReadCallback(i, &read_memory_8000_to_BFFF);
setMemoryWriteCallback(i, &write_memory_8000_to_BFFF);
}
for (uint16_t i = 0xC000; i <= 0xFBFF; i++) {
setMemoryReadCallback(i, &read_memory_C000_to_FBFF);
setMemoryWriteCallback(i, &write_memory_C000_to_FBFF);
}
for (uint16_t i = 0xFC00; i <= 0xFCFF; i++) {
setMemoryReadCallback(i, &read_memory_FC00_to_FCFF);
setMemoryWriteCallback(i, &write_memory_FC00_to_FCFF);
}
for (uint16_t i = 0xFD00; i <= 0xFEFF; i++) {
setMemoryReadCallback(i, &read_memory_FD00_to_FEFF);
setMemoryWriteCallback(i, &write_memory_FD00_to_FEFF);
}
for (uint16_t i = 0xFF00; i <= 0xFF1F; i++) {
setMemoryReadCallback(i, &read_register_FFxx);
setMemoryWriteCallback(i, &write_register_FFxx);
}
for (uint32_t i = 0xFF20; i <= 0xFFFF; i++) {
setMemoryReadCallback(uint16_t(i), &read_memory_FF00_to_FFFF);
setMemoryWriteCallback(uint16_t(i), &write_memory_FF00_to_FFFF);
}
// TED register read
setMemoryReadCallback(0x0000, &read_register_0000);
setMemoryReadCallback(0x0001, &read_register_0001);
for (uint16_t i = 0xFD00; i <= 0xFD0F; i++)
setMemoryReadCallback(i, &read_register_FD0x);
for (uint16_t i = 0xFD10; i <= 0xFD1F; i++)
setMemoryReadCallback(i, &read_register_FD1x);
setMemoryReadCallback(0xFD16, &read_register_FD16);
for (uint16_t i = 0xFD30; i <= 0xFD3F; i++)
setMemoryReadCallback(i, &read_register_FD3x);
setMemoryReadCallback(0xFF00, &read_register_FF00);
setMemoryReadCallback(0xFF01, &read_register_FF01);
setMemoryReadCallback(0xFF02, &read_register_FF02);
setMemoryReadCallback(0xFF03, &read_register_FF03);
setMemoryReadCallback(0xFF04, &read_register_FF04);
setMemoryReadCallback(0xFF05, &read_register_FF05);
setMemoryReadCallback(0xFF06, &read_register_FF06);
setMemoryReadCallback(0xFF09, &read_register_FF09);
setMemoryReadCallback(0xFF0A, &read_register_FF0A);
setMemoryReadCallback(0xFF0C, &read_register_FF0C);
setMemoryReadCallback(0xFF10, &read_register_FF10);
setMemoryReadCallback(0xFF12, &read_register_FF12);
setMemoryReadCallback(0xFF13, &read_register_FF13);
setMemoryReadCallback(0xFF14, &read_register_FF14);
setMemoryReadCallback(0xFF1A, &read_register_FF1A);
setMemoryReadCallback(0xFF1B, &read_register_FF1B);
setMemoryReadCallback(0xFF1C, &read_register_FF1C);
setMemoryReadCallback(0xFF1E, &read_register_FF1E);
setMemoryReadCallback(0xFF1F, &read_register_FF1F);
setMemoryReadCallback(0xFF3E, &read_register_FF3E_FF3F);
setMemoryReadCallback(0xFF3F, &read_register_FF3E_FF3F);
// TED register write
setMemoryWriteCallback(0x0000, &write_register_0000);
setMemoryWriteCallback(0x0001, &write_register_0001);
for (uint16_t i = 0xFD10; i <= 0xFD1F; i++)
setMemoryWriteCallback(i, &write_register_FD1x);
setMemoryWriteCallback(0xFD16, &write_register_FD16);
for (uint16_t i = 0xFD30; i <= 0xFD3F; i++)
setMemoryWriteCallback(i, &write_register_FD3x);
for (uint16_t i = 0xFDD0; i <= 0xFDDF; i++)
setMemoryWriteCallback(i, &write_register_FDDx);
setMemoryWriteCallback(0xFF00, &write_register_FF00);
setMemoryWriteCallback(0xFF01, &write_register_FF01);
setMemoryWriteCallback(0xFF02, &write_register_FF02);
setMemoryWriteCallback(0xFF03, &write_register_FF03);
setMemoryWriteCallback(0xFF04, &write_register_FF04);
setMemoryWriteCallback(0xFF05, &write_register_FF05);
setMemoryWriteCallback(0xFF06, &write_register_FF06);
setMemoryWriteCallback(0xFF07, &write_register_FF07);
setMemoryWriteCallback(0xFF08, &write_register_FF08);
setMemoryWriteCallback(0xFF09, &write_register_FF09);
setMemoryWriteCallback(0xFF0A, &write_register_FF0A);
setMemoryWriteCallback(0xFF0B, &write_register_FF0B);
setMemoryWriteCallback(0xFF0C, &write_register_FF0C);
setMemoryWriteCallback(0xFF0D, &write_register_FF0D);
setMemoryWriteCallback(0xFF0E, &write_register_FF0E);
setMemoryWriteCallback(0xFF0F, &write_register_FF0F);
setMemoryWriteCallback(0xFF10, &write_register_FF10);
setMemoryWriteCallback(0xFF11, &write_register_FF11);
setMemoryWriteCallback(0xFF12, &write_register_FF12);
setMemoryWriteCallback(0xFF13, &write_register_FF13);
setMemoryWriteCallback(0xFF14, &write_register_FF14);
setMemoryWriteCallback(0xFF15, &write_register_FF15_to_FF19);
setMemoryWriteCallback(0xFF16, &write_register_FF15_to_FF19);
setMemoryWriteCallback(0xFF17, &write_register_FF15_to_FF19);
setMemoryWriteCallback(0xFF18, &write_register_FF15_to_FF19);
setMemoryWriteCallback(0xFF19, &write_register_FF15_to_FF19);
setMemoryWriteCallback(0xFF1A, &write_register_FF1A);
setMemoryWriteCallback(0xFF1B, &write_register_FF1B);
setMemoryWriteCallback(0xFF1C, &write_register_FF1C);
setMemoryWriteCallback(0xFF1D, &write_register_FF1D);
setMemoryWriteCallback(0xFF1E, &write_register_FF1E);
setMemoryWriteCallback(0xFF1F, &write_register_FF1F);
setMemoryWriteCallback(0xFF3E, &write_register_FF3E);
setMemoryWriteCallback(0xFF3F, &write_register_FF3F);
// initialize external ports
user_port_state = uint8_t(0xFF);
tape_read_state = false;
tape_button_state = false;
// set internal TED registers
this->initRegisters();
cpu_clock_multiplier = 1;
for (int i = 0; i < 16; i++) // keyboard matrix
keyboard_matrix[i] = uint8_t(0xFF);
}
