int main (int argc, const char * argv[])
{
TwBar *myBar;
float bgColor[] = { 0.0f, 0.0f, 0.0f, 0.1f };
glm::mat4 mat;
float axis[] = { 0.7f, 0.7f, 0.7f }; // initial model rotation
float angle = 0.8f;
double FT = 0;
double FPS = 0;
double starting = 0.0;
double ending = 0.0;
int rate = 0;
int fr = 0;
zNear = 0.1f;
zFar = 100.0f;
FOV = 45.0f;
// Current time
double time = 0;
// initialise GLFW
int running = GL_TRUE;
if (!glfwInit()) {
exit(EXIT_FAILURE);
}
//only for OpenGL 2.1
#ifdef USE_OPENGL21
glfwOpenWindowHint(GLFW_OPENGL_VERSION_MAJOR, 2);
glfwOpenWindowHint(GLFW_OPENGL_VERSION_MINOR, 1);
#endif
//Only for OpenGL 3.2
#ifdef USE_OPENGL32
glfwOpenWindowHint(GLFW_FSAA_SAMPLES, 4);
glfwOpenWindowHint(GLFW_OPENGL_VERSION_MAJOR, 3);
glfwOpenWindowHint(GLFW_OPENGL_VERSION_MINOR, 2);
glfwOpenWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
glfwOpenWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
glfwOpenWindowHint(GLFW_OPENGL_DEBUG_CONTEXT, 1);
#endif
GLFWvidmode mode;
glfwGetDesktopMode(&mode);
if( !glfwOpenWindow(windowWidth, windowHeight, mode.RedBits, mode.GreenBits, mode.BlueBits, 0, 32, 0, GLFW_WINDOW /* or GLFW_FULLSCREEN */) )
{
glfwTerminate();
exit(EXIT_FAILURE);
}
glfwEnable(GLFW_MOUSE_CURSOR);
glfwEnable(GLFW_KEY_REPEAT);
// Ensure we can capture the escape key being pressed below
glfwEnable( GLFW_STICKY_KEYS );
glfwSetMousePos(windowWidth/2, windowHeight/2);
glfwSetWindowTitle("Chapter-11");
// Initialize AntTweakBar
if ( !TwInit(TW_OPENGL_CORE, NULL))
{
fprintf(stderr,"AntweakBar initialiazation failed: %s\n",TwGetLastError());
exit(1);
}
// Create a tweak bar
myBar = TwNewBar("TweakBar");
//init GLEW and basic OpenGL information
// VERY IMPORTANT OTHERWISE GLEW CANNOT HANDLE GL3
#ifdef USE_OPENGL32
glewExperimental = true;
#endif
glewInit();
std::cout<<"\nUsing GLEW "<<glewGetString(GLEW_VERSION)<<std::endl;
if (GLEW_VERSION_2_1)
{
std::cout<<"\nYay! OpenGL 2.1 is supported and GLSL 1.2!\n"<<std::endl;
}
if (GLEW_VERSION_3_2)
{
std::cout<<"Yay! OpenGL 3.2 is supported and GLSL 1.5!\n"<<std::endl;
}
/*
This extension defines an interface that allows various types of data
(especially vertex array data) to be cached in high-performance
graphics memory on the server, thereby increasing the rate of data
transfers.
Chunks of data are encapsulated within "buffer objects", which
conceptually are nothing more than arrays of bytes, just like any
chunk of memory. An API is provided whereby applications can read
from or write to buffers, either via the GL itself (glBufferData,
glBufferSubData, glGetBufferSubData) or via a pointer to the memory.
*/
if (glewIsSupported("GL_ARB_vertex_buffer_object"))
std::cout<<"ARB VBO's are supported"<<std::endl;
else if (glewIsSupported("GL_APPLE_vertex_buffer_object"))
std::cout<<"APPLE VBO's are supported"<<std::endl;
else
std::cout<<"VBO's are not supported,program will not run!!!"<<std::endl;
/*
This extension introduces named vertex array objects which encapsulate
vertex array state on the client side. The main purpose of these
objects is to keep pointers to static vertex data and provide a name
for different sets of static vertex data.
By extending vertex array range functionality this extension allows multiple
vertex array ranges to exist at one time, including their complete sets of
state, in manner analogous to texture objects.
GenVertexArraysAPPLE creates a list of n number of vertex array object
names. After creating a name, BindVertexArrayAPPLE associates the name with
a vertex array object and selects this vertex array and its associated
state as current. To get back to the default vertex array and its
associated state the client should bind to vertex array named 0.
*/
if (glewIsSupported("GL_ARB_vertex_array_object"))
std::cout<<"ARB VAO's are supported\n"<<std::endl;
else if (glewIsSupported("GL_APPLE_vertex_array_object"))//this is the name of the extension for GL2.1 in MacOSX
std::cout<<"APPLE VAO's are supported\n"<<std::endl;
else
std::cout<<"VAO's are not supported, program will not run!!!\n"<<std::endl;
std::cout<<"Vendor: "<<glGetString (GL_VENDOR)<<std::endl;
std::cout<<"Renderer: "<<glGetString (GL_RENDERER)<<std::endl;
std::cout<<"Version: "<<glGetString (GL_VERSION)<<std::endl;
std::ostringstream stream1,stream2;
stream1 << glGetString(GL_VENDOR);
stream2 << glGetString(GL_RENDERER);
std::string vendor ="Title : Chapter-11 Vendor : " + stream1.str() + " Renderer : " +stream2.str();
const char *tit = vendor.c_str();
glfwSetWindowTitle(tit);
// Set GLFW event callbacks
// - Redirect window size changes to the callback function WindowSizeCB
glfwSetWindowSizeCallback(WindowSizeCB);
// - Directly redirect GLFW mouse button events to AntTweakBar
glfwSetMouseButtonCallback((GLFWmousebuttonfun)TwEventMouseButtonGLFW);
// - Directly redirect GLFW mouse position events to AntTweakBar
glfwSetMousePosCallback((GLFWmouseposfun)TwEventMousePosGLFW);
// - Directly redirect GLFW mouse wheel events to AntTweakBar
glfwSetMouseWheelCallback((GLFWmousewheelfun)TwEventMouseWheelGLFW);
// - Directly redirect GLFW key events to AntTweakBar
glfwSetKeyCallback((GLFWkeyfun)TwEventKeyGLFW);
// - Directly redirect GLFW char events to AntTweakBar
glfwSetCharCallback((GLFWcharfun)TwEventCharGLFW);
TwDefine("TweakBar label='Main TweakBar' alpha=0 help='Use this bar to control the objects of the scene.' ");
// Add 'wire' to 'myBar': it is a modifable variable of type TW_TYPE_BOOL32 (32 bits boolean). Its key shortcut is [w].
TwAddVarRW(myBar, "wireframe mode", TW_TYPE_BOOL32, &wireFrame," label='Wireframe mode' key=w help='Toggle wireframe display mode.' ");
// Add 'bgColor' to 'myBar': it is a modifable variable of type TW_TYPE_COLOR3F (3 floats color)
TwAddVarRW(myBar, "bgColor", TW_TYPE_COLOR3F, &bgColor, " label='Background color' ");
// Add 'Rotation' to 'myBar': this is a variable of type TW_TYPE_QUAT4F which defines the scene's orientation
TwAddVarRW(myBar, "SceneRotation", TW_TYPE_QUAT4F, &Rotation," label='Scene rotation' opened=true help='Change the scenes orientation.' ");
TwAddButton(myBar, "Reset", ResetView,NULL," label='Reset View' ");
TwAddVarRW(myBar, "Near Clip Plane", TW_TYPE_FLOAT, &zNear,"min=0.5 max=100 step=0.5 label='Near Clip' group='Projection Properties'");
TwAddVarRW(myBar, "Far Clip Plane", TW_TYPE_FLOAT, &zFar," min=0.5 max=1000 step=0.5 label='Far Clip' group='Projection Properties'");
TwAddVarRW(myBar, "Field of View", TW_TYPE_FLOAT, &FOV," label='FoV' readonly=true group='Projection Properties'");
TwAddVarRW(myBar, "MS per 1 Frame" , TW_TYPE_DOUBLE, &FPS, "label='MS per 1 Frame' readonly=true group='Frame Rate'");
TwAddVarRW(myBar, "Frames Per Second" , TW_TYPE_INT32, &rate, "label='FPS' readonly=true group='Frame Rate'");
TwAddVarRW(myBar, "vSYNC" , TW_TYPE_BOOL8, &SYNC, "label='vSync' readonly=true group='Frame Rate'");
// Enable depth test
glEnable(GL_DEPTH_TEST);
// Accept fragment if it closer to the camera than the former one
glDepthFunc(GL_LESS);
initPlane(); //initialize Plane
init3Dmodel(); // initialize 3D model
create_Bump_bar();
GLfloat rat = 0.001f;
if(SYNC == false)
{
rat = 0.001f;
}
else
{
rat = 0.01f;
}
// Initialize time
time = glfwGetTime();
double currentTime;
float lastTime = 0.0f;
int Frames = 0;
double LT = glfwGetTime();
starting = glfwGetTime();
setVSync(SYNC);
while (running) {
glClear( GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT );
glClearColor( bgColor[0], bgColor[1], bgColor[2], bgColor[3]); //black color
FOV = initialFoV - 5 * glfwGetMouseWheel();
if(camera == true)
{
glfwGetMousePos(&xpos,&ypos);
glfwSetMousePos(windowWidth/2, windowHeight/2);
horizAngle += mouseSpeedo * float(windowWidth/2 - xpos );
verticAngle += mouseSpeedo * float( windowHeight/2 - ypos );
}
glm::vec3 direction(cos(verticAngle) * sin(horizAngle),sin(verticAngle),cos(verticAngle) * cos(horizAngle));
glm::vec3 right = glm::vec3(sin(horizAngle - 3.14f/2.0f),0,cos(horizAngle - 3.14f/2.0f));
glm::vec3 up = glm::cross( right, direction );
currentTime = glfwGetTime();
float dTime = float(currentTime - lastTime);
lastTime = (float)currentTime;
// Move forward
if (glfwGetKey( GLFW_KEY_UP ) == GLFW_PRESS){
pos += direction * dTime* speedo;
}
// Move backward
if (glfwGetKey( GLFW_KEY_DOWN ) == GLFW_PRESS){
pos -= direction * dTime * speedo;
}
// Strafe right
if (glfwGetKey( GLFW_KEY_RIGHT ) == GLFW_PRESS){
pos += right * dTime * speedo;
}
//Strafe left
if (glfwGetKey( GLFW_KEY_LEFT ) == GLFW_PRESS){
pos -= right * dTime * speedo;
}
if (glfwGetKey(GLFW_KEY_SPACE) == GLFW_PRESS){
if(camera == false)
{
camera=true;
glfwSetMousePos(windowWidth/2, windowHeight/2);
glfwGetMousePos(&xpos,&ypos);
}
else
{
camera=false;
glfwSetMousePos(windowWidth/2, windowHeight/2);
glfwGetMousePos(&xpos,&ypos);
}
}
mat = ConvertQuaternionToMatrix(Rotation, mat);
glm::mat4 cube;
glm::mat4 translateMat = glm::mat4();
translateMat = glm::translate(translateMat,glm::vec3(5.0,3.0,4.0));
cube = mat * translateMat;
displayPlane(mat,pos,direction,up);
display3Dmodel(cube,mat,pos,direction,up);
// drawing the AntWeakBar
if (wireFrame)
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
TwDraw();
}
else
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
TwDraw();
}
fr++;
ending = glfwGetTime();
if(ending - starting >= 1)
{
rate = fr;
fr = 0;
starting = glfwGetTime();
}
double CT = glfwGetTime();
Frames++;
if(CT -LT >= 1.0)
{
FPS = 1000.0 / (double)Frames;
Frames = 0;
LT += 1.0f;
}
glfwSwapBuffers();
//check if ESC was pressed
running=!glfwGetKey(GLFW_KEY_ESC) && glfwGetWindowParam(GLFW_OPENED);
}
//close OpenGL window and terminate AntTweakBar and GLFW
TwTerminate();
glfwTerminate();
exit(EXIT_SUCCESS);
}
void ConfigAHRSWidget::attitudeRawUpdated(UAVObject * obj)
{
QMutexLocker lock(&attitudeRawUpdateLock);
UAVObjectField *accel_field = obj->getField(QString("accels_filtered"));
UAVObjectField *gyro_field = obj->getField(QString("gyros_filtered"));
UAVObjectField *mag_field = obj->getField(QString("magnetometers"));
Q_ASSERT(gyro_field != 0 && accel_field != 0 && mag_field != 0);
// This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) {
accel_accum_x.append(accel_field->getValue(0).toDouble());
accel_accum_y.append(accel_field->getValue(1).toDouble());
accel_accum_z.append(accel_field->getValue(2).toDouble());
// Note gyros actually (-y,-x,-z) but since we consistent here no prob
mag_accum_x.append(mag_field->getValue(0).toDouble());
mag_accum_y.append(mag_field->getValue(1).toDouble());
mag_accum_z.append(mag_field->getValue(2).toDouble());
gyro_accum_x.append(gyro_field->getValue(0).toDouble());
gyro_accum_y.append(gyro_field->getValue(1).toDouble());
gyro_accum_z.append(gyro_field->getValue(2).toDouble());
}
if(accel_accum_x.size() >= 8 && collectingData == true) {
collectingData = false;
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*)));
m_ahrs->sixPointsSave->setEnabled(true);
accel_data[position] << listMean(accel_accum_x),
listMean(accel_accum_y),
listMean(accel_accum_z);
mag_data[position] << listMean(mag_accum_x),
listMean(mag_accum_y),
listMean(mag_accum_z);
gyro_data[position] << listMean(gyro_accum_x),
listMean(gyro_accum_y),
listMean(gyro_accum_z);
std::cout << "observed accel: " << accel_data[position].transpose()
<< "\nobserved mag: " << mag_data[position].transpose()
<< "\nobserved gyro: " << gyro_data[position].transpose()
<< std::endl;
struct {
const char* instructions;
const char* display;
} instructions[] = {
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Turn left 90 deg to 09:00 position and click save position...", "plane-horizontal" },
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Turn left 90 deg to 06:00 position and click save position...", "plane-horizontal" },
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Turn left 90 deg to 03:00 position and click save position...", "plane-horizontal" },
{ "Pitch up 45 deg and click save position...", "plane-horizontal" },
{ "Pitch down 45 deg and click save position...", "plane-horizontal" },
{ "Roll left 45 deg and click save position...", "plane-left" },
{ "Roll right 45 deg and click save position...", "plane-left" },
{ "Place with nose vertically up and click save position...", "plane-up" },
{ "Place with nose straight down and click save position...", "plane-down" },
{ "Place upside down and click save position...", "plane-flip" },
};
n_positions = sizeof(instructions) / sizeof(instructions[0]);
position = (position + 1) % n_positions;
if (position != 0 && position < n_positions) {
m_ahrs->sixPointCalibInstructions->append(instructions[position-1].instructions);
displayPlane(instructions[position-1].display);
}
else if(position == 0) {
position = n_positions;
computeScaleBias();
m_ahrs->sixPointsStart->setEnabled(true);
m_ahrs->sixPointsSave->setEnabled(false);
saveAHRSCalibration(); // Saves the result to SD.
/* Cleanup original settings */
getObjectManager()->getObject(QString("AttitudeRaw"))->setMetadata(initialMdata);
}
}
