<commit_before>#ifndef RADIUMENGINE_VIEWER_HPP
#define RADIUMENGINE_VIEWER_HPP

#include <GuiBase/RaGuiBase.hpp>

#include <memory>
#include <Engine/RadiumEngine.hpp>

#include <QOpenGLWidget>
#include <QThread>

#include <Core/Math/LinearAlgebra.hpp>
#include <GuiBase/Viewer/Gizmo/GizmoManager.hpp>
#include <GuiBase/Utils/FeaturePickingManager.hpp>

// Forward declarations
namespace Ra
{
    namespace Core
    {
        struct KeyEvent;
        struct MouseEvent;
    }
}

namespace Ra
{
    namespace Engine
    {
        class Renderer;
    }
}

namespace Ra
{
    namespace Gui
    {
        class CameraInterface;
        class GizmoManager;
    }
}

namespace Ra
{
    namespace Gui
    {

        // FIXME (Charly) : Which way do we want to be able to change renderers ?
        //                  Can it be done during runtime ? Must it be at startup ? ...
        //                  For now, default ForwardRenderer is used.

        /// The Viewer is the main display class. It's the central screen QWidget.
        /// Its acts as a bridge between the interface, the engine and the renderer
        /// Among its responsibilities are :
        /// * Owning the renderer and camera, and managing their lifetime.
        /// * setting up the renderer and camera by keeping it informed of interfaces changes
        ///  (e.g. resize).
        /// * catching user interaction (mouse clicks) at the lowest level and forward it to
        /// the camera and the rest of the application
        /// * Expose the asynchronous rendering interface
        class RA_GUIBASE_API Viewer : public QOpenGLWidget
        {
            Q_OBJECT

        public:
            /// Constructor
            explicit Viewer( QWidget* parent = nullptr );

            /// Destructor
            ~Viewer();

            //
            // Accessors
            //

            /// Access to camera interface.
            CameraInterface* getCameraInterface();

            /// Access to gizmo manager
            GizmoManager* getGizmoManager();

            /// Read-only access to renderer
            const Engine::Renderer* getRenderer() const;

            /// Read-write access to renderer
            Engine::Renderer* getRenderer();

            /// Access to the feature picking manager
            FeaturePickingManager* getFeaturePickingManager();

            //
            // Rendering management
            //

            /// Start rendering (potentially asynchronously in a separate thread)
            void startRendering( const Scalar dt );

            /// Blocks until rendering is finished.
            void waitForRendering();

            //
            // Misc functions
            //

            /// Load data from a file.
            void handleFileLoading( const std::string& file );

            /// Load data from a FileData.
            void handleFileLoading( const Ra::Asset::FileData &filedata );

            /// Emits signals corresponding to picking requests.
            void processPicking();

            /// Moves the camera so that the whole scene is visible.
            void fitCameraToScene( const Core::Aabb& sceneAabb );

            /// Returns the names of the different registred renderers.
            std::vector<std::string> getRenderersName() const;

            /// Write the current frame as an image. Supports either BMP or PNG file names.
            void grabFrame( const std::string& filename );

        signals:
            void glInitialized();               //! Emitted when GL context is ready. We except call to addRenderer here
            void rendererReady();               //! Emitted when the rendered is correctly initialized
            void leftClickPicking ( int id );   //! Emitted when the result of a left click picking is known
            void rightClickPicking( int id );   //! Emitted when the resut of a right click picking is known

        public slots:
            /// Tell the renderer to reload all shaders.
            void reloadShaders();

            /// Set the final display texture
            void displayTexture( const QString& tex );

            /// Set the renderer
            void changeRenderer( int index );

            /// Toggle the post-process effetcs
            void enablePostProcess(int enabled);

            /// Toggle the debug drawing
            void enableDebugDraw(int enabled);

            /// Resets the camaera to initial values
            void resetCamera();

            /** Add a renderer and return its index. Need to be called when catching
             * \param e : unique_ptr to your own renderer
             * \return index of the newly added renderer
             * \code
             * int rendererId = addRenderer(new MyRenderer(width(), height()));
             * changeRenderer(rendererId);
             * getRenderer()->initialize();
             * auto light = Ra::Core::make_shared<Engine::DirectionalLight>();
             * getRenderer()->addLight( light );
             * m_camera->attachLight( light );
             * \endcode
             */
            int addRenderer(std::shared_ptr<Engine::Renderer> e);

        private slots:
            /// These slots are connected to the base class signals to properly handle
            /// concurrent access to the renderer.
            void onAboutToCompose();
            void onAboutToResize();
            void onFrameSwapped();
            void onResized();

        protected:
            /// Initialize renderer internal state + configure lights.
            void intializeRenderer(Engine::Renderer* renderer);

            //
            // QOpenGlWidget primitives
            //

            /// Initialize openGL. Called on by the first "show" call to the main window.
            virtual void initializeGL() override;

            /// Resize the view port and the camera. Called by the resize event.
            virtual void resizeGL( int width, int height ) override;

            /// Paint event is set to a no-op to prevent synchronous rendering.
            /// We don't implement paintGL as well.
            virtual void paintEvent( QPaintEvent* e ) override {}

            //
            // Qt input events.
            //

            virtual void keyPressEvent( QKeyEvent* event ) override;
            virtual void keyReleaseEvent( QKeyEvent* event ) override;

            /// We intercept the mouse events in this widget to get the coordinates of the mouse
            /// in screen space.
            virtual void mousePressEvent( QMouseEvent* event ) override;
            virtual void mouseReleaseEvent( QMouseEvent* event ) override;
            virtual void mouseMoveEvent( QMouseEvent* event ) override;
            virtual void wheelEvent( QWheelEvent* event ) override;

        public:
            Scalar m_dt;

        protected:
            /// Owning pointer to the renderers.
            std::vector<std::shared_ptr<Engine::Renderer>> m_renderers;
            Engine::Renderer* m_currentRenderer;

            /// Owning Pointer to the feature picking manager.
            FeaturePickingManager* m_featurePickingManager;

            /// Owning pointer to the camera.
            std::unique_ptr<CameraInterface> m_camera;

            /// Owning (QObject child) pointer to gizmo manager.
            GizmoManager* m_gizmoManager;

            /// Thread in which rendering is done.
            QThread* m_renderThread; // We have to use a QThread for MT rendering

            /// GL initialization status
            std::atomic_bool m_glInitStatus;
        };

    } // namespace Gui
} // namespace Ra

#endif // RADIUMENGINE_VIEWER_HPP
<commit_msg>Add missing include ;<commit_after>#ifndef RADIUMENGINE_VIEWER_HPP
#define RADIUMENGINE_VIEWER_HPP

#include <GuiBase/RaGuiBase.hpp>

#include <atomic>
#include <memory>
#include <Engine/RadiumEngine.hpp>

#include <QOpenGLWidget>
#include <QThread>

#include <Core/Math/LinearAlgebra.hpp>
#include <GuiBase/Viewer/Gizmo/GizmoManager.hpp>
#include <GuiBase/Utils/FeaturePickingManager.hpp>

// Forward declarations
namespace Ra
{
    namespace Core
    {
        struct KeyEvent;
        struct MouseEvent;
    }
}

namespace Ra
{
    namespace Engine
    {
        class Renderer;
    }
}

namespace Ra
{
    namespace Gui
    {
        class CameraInterface;
        class GizmoManager;
    }
}

namespace Ra
{
    namespace Gui
    {

        // FIXME (Charly) : Which way do we want to be able to change renderers ?
        //                  Can it be done during runtime ? Must it be at startup ? ...
        //                  For now, default ForwardRenderer is used.

        /// The Viewer is the main display class. It's the central screen QWidget.
        /// Its acts as a bridge between the interface, the engine and the renderer
        /// Among its responsibilities are :
        /// * Owning the renderer and camera, and managing their lifetime.
        /// * setting up the renderer and camera by keeping it informed of interfaces changes
        ///  (e.g. resize).
        /// * catching user interaction (mouse clicks) at the lowest level and forward it to
        /// the camera and the rest of the application
        /// * Expose the asynchronous rendering interface
        class RA_GUIBASE_API Viewer : public QOpenGLWidget
        {
            Q_OBJECT

        public:
            /// Constructor
            explicit Viewer( QWidget* parent = nullptr );

            /// Destructor
            ~Viewer();

            //
            // Accessors
            //

            /// Access to camera interface.
            CameraInterface* getCameraInterface();

            /// Access to gizmo manager
            GizmoManager* getGizmoManager();

            /// Read-only access to renderer
            const Engine::Renderer* getRenderer() const;

            /// Read-write access to renderer
            Engine::Renderer* getRenderer();

            /// Access to the feature picking manager
            FeaturePickingManager* getFeaturePickingManager();

            //
            // Rendering management
            //

            /// Start rendering (potentially asynchronously in a separate thread)
            void startRendering( const Scalar dt );

            /// Blocks until rendering is finished.
            void waitForRendering();

            //
            // Misc functions
            //

            /// Load data from a file.
            void handleFileLoading( const std::string& file );

            /// Load data from a FileData.
            void handleFileLoading( const Ra::Asset::FileData &filedata );

            /// Emits signals corresponding to picking requests.
            void processPicking();

            /// Moves the camera so that the whole scene is visible.
            void fitCameraToScene( const Core::Aabb& sceneAabb );

            /// Returns the names of the different registred renderers.
            std::vector<std::string> getRenderersName() const;

            /// Write the current frame as an image. Supports either BMP or PNG file names.
            void grabFrame( const std::string& filename );

        signals:
            void glInitialized();               //! Emitted when GL context is ready. We except call to addRenderer here
            void rendererReady();               //! Emitted when the rendered is correctly initialized
            void leftClickPicking ( int id );   //! Emitted when the result of a left click picking is known
            void rightClickPicking( int id );   //! Emitted when the resut of a right click picking is known

        public slots:
            /// Tell the renderer to reload all shaders.
            void reloadShaders();

            /// Set the final display texture
            void displayTexture( const QString& tex );

            /// Set the renderer
            void changeRenderer( int index );

            /// Toggle the post-process effetcs
            void enablePostProcess(int enabled);

            /// Toggle the debug drawing
            void enableDebugDraw(int enabled);

            /// Resets the camaera to initial values
            void resetCamera();

            /** Add a renderer and return its index. Need to be called when catching
             * \param e : unique_ptr to your own renderer
             * \return index of the newly added renderer
             * \code
             * int rendererId = addRenderer(new MyRenderer(width(), height()));
             * changeRenderer(rendererId);
             * getRenderer()->initialize();
             * auto light = Ra::Core::make_shared<Engine::DirectionalLight>();
             * getRenderer()->addLight( light );
             * m_camera->attachLight( light );
             * \endcode
             */
            int addRenderer(std::shared_ptr<Engine::Renderer> e);

        private slots:
            /// These slots are connected to the base class signals to properly handle
            /// concurrent access to the renderer.
            void onAboutToCompose();
            void onAboutToResize();
            void onFrameSwapped();
            void onResized();

        protected:
            /// Initialize renderer internal state + configure lights.
            void intializeRenderer(Engine::Renderer* renderer);

            //
            // QOpenGlWidget primitives
            //

            /// Initialize openGL. Called on by the first "show" call to the main window.
            virtual void initializeGL() override;

            /// Resize the view port and the camera. Called by the resize event.
            virtual void resizeGL( int width, int height ) override;

            /// Paint event is set to a no-op to prevent synchronous rendering.
            /// We don't implement paintGL as well.
            virtual void paintEvent( QPaintEvent* e ) override {}

            //
            // Qt input events.
            //

            virtual void keyPressEvent( QKeyEvent* event ) override;
            virtual void keyReleaseEvent( QKeyEvent* event ) override;

            /// We intercept the mouse events in this widget to get the coordinates of the mouse
            /// in screen space.
            virtual void mousePressEvent( QMouseEvent* event ) override;
            virtual void mouseReleaseEvent( QMouseEvent* event ) override;
            virtual void mouseMoveEvent( QMouseEvent* event ) override;
            virtual void wheelEvent( QWheelEvent* event ) override;

        public:
            Scalar m_dt;

        protected:
            /// Owning pointer to the renderers.
            std::vector<std::shared_ptr<Engine::Renderer>> m_renderers;
            Engine::Renderer* m_currentRenderer;

            /// Owning Pointer to the feature picking manager.
            FeaturePickingManager* m_featurePickingManager;

            /// Owning pointer to the camera.
            std::unique_ptr<CameraInterface> m_camera;

            /// Owning (QObject child) pointer to gizmo manager.
            GizmoManager* m_gizmoManager;

            /// Thread in which rendering is done.
            QThread* m_renderThread; // We have to use a QThread for MT rendering

            /// GL initialization status
            std::atomic_bool m_glInitStatus;
        };

    } // namespace Gui
} // namespace Ra

#endif // RADIUMENGINE_VIEWER_HPP
<|endoftext|>


<commit_before>// BookKeeper.cpp
// Implements the BookKeeper class

#include "BookKeeper.h"

#include <string>
#include <iostream>
#include <fstream>
#include <stdlib.h>

#include "Database.h"
#include "Table.h"
#include "CycException.h"

BookKeeper* BookKeeper::instance_ = 0;
bool BookKeeper::logging_on_ = true;

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
BookKeeper* BookKeeper::Instance() {
  // If we haven't created a BookKeeper yet, create and return it.
  if (0 == instance_){
    instance_ = new BookKeeper();
  }
  return instance_;
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
BookKeeper::BookKeeper() {
  dbIsOpen_ = false;
  dbExists_ = false;
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool BookKeeper::fexists(const char *filename) {
  std::ifstream ifile(filename);
  return ifile;
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::createDB(std::string name) {
  dbName_ = name;
  try{
    // get database name and location
    char* oPath = getenv("CYCLUS_OUTPUT_DIR");
    if (oPath==NULL) {
      std::string err = std::string("Cyclus output path - envrionment ")
	+ std::string("variable: CYCLUS_OUTPUT_DIR - not defined");
      throw CycIOException(err);
    }

    // construct output file path
    std::string out_path(oPath);
    std::string db_path = out_path + "/" + name;

    // if the file already exists, delete it
    if( fexists( db_path.c_str() ) )
      remove( db_path.c_str() );

    // create database.
    db_ = new Database(name);
    dbIsOpen_ = true;
    dbExists_ = true;
  }
  catch( CycException& error ) {
    // just throw it back for now
    // @MJG flag, do we need to change this?
    throw CycException( std::string(error.what()) );
  }
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::turnOffLogging() {
  logging_on_ = false;
  if ( db_->nTables() > 0 ) {
    std::string err =
      "Logging can not be turned off once a table has already been created.";
    throw CycException();
  }
}

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::registerTable(table_ptr t) {
  if ( loggingIsOn() ){
    db_->registerTable(t);
    db_->createTable(t);
  }
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::tableAtThreshold(table_ptr t) {
  if ( loggingIsOn() ){
    db_->writeRows(t);
    t->flush();
  }
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::closeDB() {
  // have the database print and remaining commands
  for (int i = 0; i < db_->nTables(); i++) {
    table_ptr t = db_->tablePtr(i);
    if (t->nRows() > 0)
      this->tableAtThreshold(t);
  }
  // close the db
  db_->close();
}
<commit_msg>more redundancy for database existance checking.<commit_after>// BookKeeper.cpp
// Implements the BookKeeper class

#include "BookKeeper.h"

#include <string>
#include <iostream>
#include <fstream>
#include <stdlib.h>

#include "Database.h"
#include "Table.h"
#include "CycException.h"

BookKeeper* BookKeeper::instance_ = 0;
bool BookKeeper::logging_on_ = true;

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
BookKeeper* BookKeeper::Instance() {
  // If we haven't created a BookKeeper yet, create and return it.
  if (0 == instance_){
    instance_ = new BookKeeper();
  }
  return instance_;
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
BookKeeper::BookKeeper() {
  dbIsOpen_ = false;
  dbExists_ = false;
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bool BookKeeper::fexists(const char *filename) {
  std::ifstream ifile(filename);
  return ifile;
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::createDB(std::string name) {
  dbName_ = name;
  try{
    // get database name and location
    char* oPath = getenv("CYCLUS_OUTPUT_DIR");
    if (oPath==NULL) {
      std::string err = std::string("Cyclus output path - envrionment ")
	+ std::string("variable: CYCLUS_OUTPUT_DIR - not defined");
      throw CycIOException(err);
    }

    // construct output file path
    std::string out_path(oPath);
    std::string db_path = out_path + "/" + name;

    // if the file already exists, delete it
    if( fexists( db_path.c_str() ) )
      remove( db_path.c_str() );

    // create database.
    db_ = new Database(name);
    if ( db_->dbExists() ) {
      dbIsOpen_ = true;
      dbExists_ = true;
    }
  }
  catch( CycException& error ) {
    // just throw it back for now
    // @MJG flag, do we need to change this?
    throw CycException( std::string(error.what()) );
  }
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::turnOffLogging() {
  logging_on_ = false;
  if ( db_->nTables() > 0 ) {
    std::string err =
      "Logging can not be turned off once a table has already been created.";
    throw CycException();
  }
}

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::registerTable(table_ptr t) {
  if ( loggingIsOn() ){
    db_->registerTable(t);
    db_->createTable(t);
  }
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::tableAtThreshold(table_ptr t) {
  if ( loggingIsOn() ){
    db_->writeRows(t);
    t->flush();
  }
};

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void BookKeeper::closeDB() {
  // have the database print and remaining commands
  for (int i = 0; i < db_->nTables(); i++) {
    table_ptr t = db_->tablePtr(i);
    if (t->nRows() > 0)
      this->tableAtThreshold(t);
  }
  // close the db
  db_->close();
}
<|endoftext|>


<commit_before>/*
* The MIT License (MIT)
*
* Copyright (c) 2015 vmolsa <ville.molsa@gmail.com> (http://github.com/vmolsa)
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
*/

#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <iostream>
#include <sys/time.h>

#include "WindowRenderer.h"

#include "talk/media/devices/videorendererfactory.h"

//using namespace v8;
using namespace WebRTC;

int WindowRenderer::StreamId = 0;

WindowRenderer::WindowRenderer(v8::Local<v8::Object> properties) :
  _id(WindowRenderer::StreamId++),
  _width(600),
  _height(480),
  _window(0),
  _fullScreen(false),
  _module(0),
  _type(webrtc::kRenderDefault),
  _renderer(0)
{
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  EventEmitter::SetReference(true);
  const char *error = 0;

  int screen;
  XEvent event;
  XSetWindowAttributes attr;
  XVisualInfo info;
  Window window = 0;
  Display* display = XOpenDisplay(NULL);

  if (display) {
    screen = DefaultScreen(display);

    (void) XMatchVisualInfo(display, screen, 24, TrueColor, &info);

    attr.colormap = XCreateColormap(display, DefaultRootWindow(display), info.visual, AllocNone);
    attr.event_mask = StructureNotifyMask | ExposureMask;
    attr.background_pixel = 0;
    attr.border_pixel = 0;

    mask = CWBackPixel | CWBorderPixel | CWColormap | CWEventMask;

    window = XCreateWindow(display, DefaultRootWindow(display), 0, 0, _width, _height, 0, info.depth, InputOutput, info.visual, mask, &attr);
  } else {
    error = "Unable to connect X11";
  }

  if (window) {
    XStoreName(display, window, "WebRTC @ NodeJS");
    XSetIconName(display, window, "WebRTC @ NodeJS");
    XSelectInput(display, window, StructureNotifyMask);
    XMapWindow(display, window);

    do {
        XNextEvent(display, &event);
    } while (event.type != MapNotify || event.xmap.event != window);

    _window = window;
    _type = webrtc::kRenderX11;
    _module = webrtc::VideoRender::CreateVideoRender(1337, _window, _fullScreen, _type);
  }

  if (_module) {
    _renderer = _module->AddIncomingRenderStream(_id, 0, 0.0f, 0.0f, 1.0f, 1.0f);

    if (_renderer) {
      if (_module->StartRender(_id)) {
        error = "Unable to start renderer";
      }
    } else {
      error = "Unable to create renderer";
    }
  } else {
    error = "Unable to create window";
  }

  if (error) {
    NanThrowError(error);
  }
}

WindowRenderer::~WindowRenderer() {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  WindowRenderer::End();
}

void WindowRenderer::Init() {

}

void WindowRenderer::Init(v8::Local<v8::Object> constructor) {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  NanScope();

  Local<FunctionTemplate> tpl = NanNew<FunctionTemplate>(WindowRenderer::New);
  constructor->Set(NanNew("window"), tpl->GetFunction());
}

NAN_METHOD(WindowRenderer::New) {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  NanScope();

  WindowRenderer* renderer = new WindowRenderer(v8::Local<v8::Object>::Cast(args[0]));
  renderer->Wrap(args.This(), "MediaSource");
  NanReturnValue(args.This());
}

void WindowRenderer::End() {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  if (_module && _renderer) {
    _module->StopRender(_id);
    _module->DeleteIncomingRenderStream(_id);

    webrtc::VideoRender::DestroyVideoRender(_module);

    _renderer = 0;
    _module = 0;
  }

  MediaSource::End();
}

void WindowRenderer::On(Event *event) {
  MediaSourceEvent type = event->Type<MediaSourceEvent>();
  webrtc::VideoFrame frame;

  if (type == kMediaSourceFrame) {
    frame.set_video_frame_buffer(event->Unwrap<rtc::scoped_refptr<webrtc::VideoFrameBuffer> >());

    if (_renderer) {
      _renderer->RenderFrame(_id, frame);
    }
  } else if (type == kMediaSourceEnd) {
    WindowRenderer::End();
  }
}<commit_msg>Update WindowRenderer<commit_after>/*
* The MIT License (MIT)
*
* Copyright (c) 2015 vmolsa <ville.molsa@gmail.com> (http://github.com/vmolsa)
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
*/

#include "WindowRenderer.h"
#include "talk/media/devices/videorendererfactory.h"

#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <iostream>
#include <sys/time.h>

//using namespace v8;
using namespace WebRTC;

int WindowRenderer::StreamId = 0;

WindowRenderer::WindowRenderer(v8::Local<v8::Object> properties) :
  _id(WindowRenderer::StreamId++),
  _width(600),
  _height(480),
  _window(0),
  _fullScreen(false),
  _module(0),
  _type(webrtc::kRenderDefault),
  _renderer(0)
{
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  EventEmitter::SetReference(true);
  const char *error = 0;

  int screen;
  XEvent event;
  XSetWindowAttributes attr;
  XVisualInfo info;
  Window window = 0;
  Display* display = XOpenDisplay(NULL);

  if (display) {
    screen = DefaultScreen(display);

    (void) XMatchVisualInfo(display, screen, 24, TrueColor, &info);

    attr.colormap = XCreateColormap(display, DefaultRootWindow(display), info.visual, AllocNone);
    attr.event_mask = StructureNotifyMask | ExposureMask;
    attr.background_pixel = 0;
    attr.border_pixel = 0;

    mask = CWBackPixel | CWBorderPixel | CWColormap | CWEventMask;

    window = XCreateWindow(display, DefaultRootWindow(display), 0, 0, _width, _height, 0, info.depth, InputOutput, info.visual, mask, &attr);
  } else {
    error = "Unable to connect X11";
  }

  if (window) {
    XStoreName(display, window, "WebRTC @ NodeJS");
    XSetIconName(display, window, "WebRTC @ NodeJS");
    XSelectInput(display, window, StructureNotifyMask);
    XMapWindow(display, window);

    do {
        XNextEvent(display, &event);
    } while (event.type != MapNotify || event.xmap.event != window);

    _window = window;
    _type = webrtc::kRenderX11;
    _module = webrtc::VideoRender::CreateVideoRender(1337, _window, _fullScreen, _type);
  }

  if (_module) {
    _renderer = _module->AddIncomingRenderStream(_id, 0, 0.0f, 0.0f, 1.0f, 1.0f);

    if (_renderer) {
      if (_module->StartRender(_id)) {
        error = "Unable to start renderer";
      }
    } else {
      error = "Unable to create renderer";
    }
  } else {
    error = "Unable to create window";
  }

  if (error) {
    NanThrowError(error);
  }
}

WindowRenderer::~WindowRenderer() {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  WindowRenderer::End();
}

void WindowRenderer::Init() {

}

void WindowRenderer::Init(v8::Local<v8::Object> constructor) {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  NanScope();

  Local<FunctionTemplate> tpl = NanNew<FunctionTemplate>(WindowRenderer::New);
  constructor->Set(NanNew("window"), tpl->GetFunction());
}

NAN_METHOD(WindowRenderer::New) {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  NanScope();

  WindowRenderer* renderer = new WindowRenderer(v8::Local<v8::Object>::Cast(args[0]));
  renderer->Wrap(args.This(), "MediaSource");
  NanReturnValue(args.This());
}

void WindowRenderer::End() {
  LOG(LS_INFO) << __PRETTY_FUNCTION__;

  if (_module && _renderer) {
    _module->StopRender(_id);
    _module->DeleteIncomingRenderStream(_id);

    webrtc::VideoRender::DestroyVideoRender(_module);

    _renderer = 0;
    _module = 0;
  }

  MediaSource::End();
}

void WindowRenderer::On(Event *event) {
  MediaSourceEvent type = event->Type<MediaSourceEvent>();
  webrtc::VideoFrame frame;

  if (type == kMediaSourceFrame) {
    frame.set_video_frame_buffer(event->Unwrap<rtc::scoped_refptr<webrtc::VideoFrameBuffer> >());

    if (_renderer) {
      _renderer->RenderFrame(_id, frame);
    }
  } else if (type == kMediaSourceEnd) {
    WindowRenderer::End();
  }
}<|endoftext|>


<commit_before>/******************************************************************************
 *	Main script for the 2017 RoboFishy Scripps AUV
 *****************************************************************************/

#include "Mapper.h"

// Multithreading //
#include <pthread.h>
#include <sched.h>
#include <unistd.h>

// Sampling Values //
#define SAMPLE_RATE 200 // sample rate of main control loop (Hz)
#define DT 0.005		// timestep; make sure this is equal to 1/SAMPLE_RATE!

// Conversion Factors //
#define UNITS_KPA 0.1 // converts pressure from mbar to kPa

/******************************************************************************
 * Controller Gains
******************************************************************************/

// Yaw Controller //
#define KP_YAW 0.01
#define KI_YAW 0
#define KD_YAW 1

// Depth Controller //
#define KP_DEPTH 0
#define KI_DEPTH 0
#define KD_DEPTH 0

// Saturation Constants //
#define YAW_SAT 1		// upper limit of yaw controller
#define DEPTH_SAT 1		// upper limit of depth controller
#define INT_SAT 10		// upper limit of integral windup
#define DINT_SAT 10		// upper limit of depth integral windup

// Filter Values //
#define A1 0.3			// for MS5837 pressure sensor
#define A2 0.4			// for MS5837 pressure sensor

// Fluid Densities in kg/m^3 //
#define DENSITY_FRESHWATER 997
#define DENSITY_SALTWATER 1029

// Acceleration Due to Gravity in m/s^2 //
#define GRAVITY 9.81

// Depth Start Value //
#define DEPTH_START 50 // starting depth (mm)

// Depth Threshold Value //
#define DEPTH_STOP 10000	// threshold depth (mm)

// Temperature Threshold Value //
#define TEMP_STOP 50	// deg C

// Stop Timer //
#define STOP_TIME 4		// seconds

// SOS Leak Sensor Pin //
#define SOSPIN 27		// connected to GPIO 27

/******************************************************************************
 * Declare Threads
******************************************************************************/

void *navigation(void* arg);
void *depth_thread(void* arg);
void *safety_thread(void* arg);

/******************************************************************************
 * Global Variables
******************************************************************************/

// holds the setpoint data structure with current setpoints
setpoint_t setpoint;

// holds the system state structure with current system statesystem_state_t sstate;
system_state_t sstate;

// holds the calibration values for the MS5837 pressure sensor
pressure_calib_t pressure_calib;

// holds the latest pressure value from the MS5837 pressure sensor
ms5837_t ms5837;

// create structure for storing IMU data
bno055_t bno055;

// holds the latest temperature value from the DS18B20 temperature sensor
ds18b20_t ds18b20;

// holds the constants and latest errors of the yaw pid controller
pid_data_t yaw_pid;

// holds the constants and latest errors of the depth pid controller
pid_data_t depth_pid;

// motor channels
int motor_channels[] = {CHANNEL_1, CHANNEL_2, CHANNEL_3};

// Ignoring sstate
float depth = 0;

/******************************************************************************
* Main Function
******************************************************************************/

int main()
{
	// Initialize python interpreter
	Py_Initialize();

	//Set up Pi GPIO pins through wiringPi
	wiringPiSetupGpio();

	// Check if AUV is initialized correctly //
	if(scripps_auv_init()<0)
	{
		return -1;
	}
	printf("\nAll components are initializated\n");
	substate.mode = INITIALIZING;

	// Initialize threads
	sched_param param;
	int policy, maxpriority;

	// Initialize priorities
	pthread_attr_init(&tattrlow);
	pthread_attr_init(&tattrmed);
	pthread_attr_init(&tattrhigh);

	// Get max priority
	pthread_attr_getschedpolicy(&tattrlow, &policy);
	maxpriority = sched_get_priority_max(policy);

	// Extract scheduling parameter
	pthread_attr_getschedparam (&tattrlow, &param);

	// Set up low priority
	param.sched_priority = maxpriority/4;
	pthread_attr_setschedparam (&tattrlow, &param);

	// Set up medium priority
	param.sched_priority = maxpriority/2;
	pthread_attr_setschedparam (&tattrmed, &param);

	// Set up high priority
	param.sched_priority = maxpriority-1;
	pthread_attr_setschedparam (&tattrhigh, &param);

	// Thread handles
	pthread_t navigationThread;
	pthread_t depthThread;
	pthread_t safetyThread;

	// Create threads using modified attributes
	pthread_create (&navigationThread, &tattrmed, navigation, NULL);
//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
//	pthread_create (&safetyThread, &tattrlow, safety_thread, NULL);

//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);

	// Destroy the thread attributes
	pthread_attr_destroy(&tattrlow);
	pthread_attr_destroy(&tattrmed);
	pthread_attr_destroy(&tattrhigh);

	// Start timer!
	time_t start = time(0);

	// Run main while loop, wait until it's time to stop
	while(substate.mode != STOPPED)
	{
		// Check if we've passed the stop time
		if(difftime(time(0),start) > STOP_TIME)
			substate.mode = STOPPED;

		// Sleep a little
		usleep(100000);
	}

	// Exit cleanly
	cleanup_auv();
	return 0;
}

/******************************************************************************
* Depth Thread
*
* For Recording Depth & Determining If AUV is in Water or Not
******************************************************************************/
void *depth_thread(void* arg)
{
	// initialize pressure sensor //
	pressure_calib = init_ms5837();

	while(substate.mode!=STOPPED)
	{
		// read pressure sensor by passing calibration structure //
		ms5837 = ms5837_read(pressure_calib);

		// calculate depth (no idea what the magic numbers are)
		depth = (ms5837.pressure-1013)*10.197-88.8; // units?
		// 1013: ambient pressure (mbar)

		usleep(10000);
	}

	pthread_exit(NULL);
}

/******************************************************************************
 * Navigation Thread
 *
 * For yaw control
 *****************************************************************************/

void *navigation(void* arg)
{
	initialize_motors(motor_channels, HERTZ);

	//float output_port;		// port motor output
	//float output_starboard; // starboard motor output

	// initialize Motor Percent to be returned by yaw_controller //
	float motor_percent;

	// initialize old imu data //
	yaw_pid.old = 0;

	// initialize setpoint for yaw_controller //
	yaw_pid.setpoint = 0;

	// initialize error values to be used in yaw_controller //
	yaw_pid.err = 0;
	yaw_pid.i_err = 0;

	// yaw_controller constant initialization ///
	yaw_pid.kp = 0.01;
	yaw_pid.kd = 0;
	yaw_pid.ki = 0;

	// range-from-bottom setpoint //
	depth_pid.setpoint = 2;	// meters

	// depth controller constant initialization
	depth_pid.kp = 0.01;
	depth_pid.kd = 0;
	depth_pid.ki = 0;

	// hard set motor speed //
	// pwmWrite(PIN_BASE+motor_channels[1], output_starboard)
	set_motor(0, -0.2);  // right
	set_motor(1, 0.2); // left
    set_motor(2, 0.0);

	while(substate.mode!=STOPPED)
	{
		// read IMU values
	bno055 = bno055_read();

    if (bno055.yaw < 180)
	{
	yaw_pid.err = abs(bno055.yaw - yaw_pid.setpoint);
	}
	else
	{
	yaw_pid.err =abs((bno055.yaw-360) - yaw_pid.setpoint);
	}
	// Write captured values to screen
    /*printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
				 bno055.yaw, bno055.pitch, bno055.roll,
				 bno055.p, bno055.q, bno055.r,
				 bno055.sys, bno055.gyro, bno055.accel,
				 bno055.mag);*/
	printf("\nYawPID_err: %f Motor Percent: %f ", yaw_pid.err, motor_percent);

	// Sanity test: Check if yaw control works
	/*
	//Call yaw controller function
	yaw_controller();

	//set port motor
	set_motors(0,motor_percent);

	//set starboard motor
	set_motors(1, motor_percent);

	*/

	// sleep for 5 ms //
		usleep(5000);
	}

	set_motor(0, 0);
	set_motor(1, 0);
    set_motor(2, 0);

	pthread_exit(NULL);
}

/******************************************************************************
 * Safety Thread
 *
 * Shuts down AUV if vehicle goes belows 10m, temperature gets too high, or
 * water intrusion is detected
 *****************************************************************************/
void *safety_thread(void* arg)
{
	// set up WiringPi for use // (not sure if actually needed)
	wiringPiSetup();

	// leak detection variables //
	int leakStatePin = digitalRead(SOSPIN);	// read the input pin
	int i;									// loop counting integer

	// leak detection pins //
	pinMode(SOSPIN, INPUT);					// set SOSPIN as an INPUT
	pinMode(17, OUTPUT);					// set GPIO 17 as an OUTPUT
	digitalWrite(leakStatePin, HIGH);		// set GPIO 17 as HIGH (VCC)

	while( substate.mode!=STOPPED )
	{
		/******************************************************************************
		 * Depth Protection
		 *
		 * Shut down AUV if vehicle travels deeper than 10m
		 *****************************************************************************/

		// get pressure value //
		pressure_calib = init_ms5837();
		ms5837 = ms5837_read(pressure_calib);
		sstate.depth[0] = (ms5837.pressure-1013)*10.197-88.8;
		//sstate.depth[0] = (ms5837.pressure*UNITS_KPA-101.325)/
		//	(DENSITY_FRESHWATER*GRAVITY)*0.000001;		// current depth in mm
		printf("%f\n", sstate.depth[0]);

		// filtered depth value //
		sstate.fdepth[0] = A1*(sstate.depth[0]+sstate.depth[1])+A2*sstate.fdepth[1];
		printf("Current depth is %f\n m", sstate.fdepth[0]/1000);

		//if( sstate.fdepth[0]< DEPTH_STOP )
		if( sstate.depth[0] < DEPTH_STOP )
		{
			substate.mode = RUNNING;
		}
		else
		{
			substate.mode = STOPPED;
			printf("%s\n", "STOPPED");
		}

		// set current depth values as old values //
		sstate.depth[1] = sstate.depth[0];
		sstate.fdepth[1] = sstate.fdepth[0];

		// sleep for 50 ms //
		usleep(50000);

		/******************************************************************************
		 * Temperature Protection
		 *
		 * Shut down AUV if housing temperature exceeds 50 deg C
		 *****************************************************************************/

		// read temperature values from DS18B20 temperature sensor //
		ds18b20 = ds18b20_read(); 	// temperature in deg C

		// let 'em know how hot we are //
		printf("Temperature: %f", ds18b20.temperature);

		// turn motors off if housing temperature gets too high //
		if( ds18b20.temperature > TEMP_STOP )
		{
			for( i=0; i<3; i++ )
			{
				pwmWrite(PIN_BASE+i, MOTOR_0);
			}
		}

		/******************************************************************************
		 * Leak Protection
		 *
		 * Shut down AUV if a leak is detected
		 *****************************************************************************/

		// check leak sensor for water intrusion //
		if( leakStatePin == HIGH )
		{
			// tell 'em it's bad //
			printf("LEAK DETECTED!\n");
			for( i=0; i<3; i++ )
			{
				// shut off motors //
				set_motor(PIN_BASE+i, MOTOR_0);
			}
        }
		else if (leakStatePin == LOW)
		{
			// tell 'em we're dry //
			printf("We're dry.\n");
		}

		pthread_exit(NULL);
	}

    return NULL;
}

/******************************************************************************
 * Logging Thread
 *
 * Logs the sensor output data into a file
 *****************************************************************************/
/*
PI_THREAD (logging_thread)
{
	while(substate.mode!=STOPPED){
		FILE *fd = fopen("log.txt", "a");
		char buffer[100] = {0};
		// add logging values to the next line
		sprintf(buffer, "%f %f %f %f %i %i %i %i %f %f %f %f\n",sstate.roll, sstate.pitch[0], sstate.yaw[0], sstate.depth[0],sstate.x[0],
		sstate.y[0], sstate.radius[0], setpoint.x - sstate.x[0], sstate.esc_out[0], sstate.esc_out[1], sstate.esc_out[2], sstate.esc_out[3]);
		fputs(buffer, fd);
		fclose(fd);
		//sleep for 100 ms

		usleep(100000);
	}
	return 0;
}
*/
<commit_msg>Controls debugging<commit_after>/******************************************************************************
 *	Main script for the 2017 RoboFishy Scripps AUV
 *****************************************************************************/

#include "Mapper.h"

// Multithreading //
#include <pthread.h>
#include <sched.h>
#include <unistd.h>

// Sampling Values //
#define SAMPLE_RATE 200 // sample rate of main control loop (Hz)
#define DT 0.005		// timestep; make sure this is equal to 1/SAMPLE_RATE!

// Conversion Factors //
#define UNITS_KPA 0.1 // converts pressure from mbar to kPa

/******************************************************************************
 * Controller Gains
******************************************************************************/

// Yaw Controller //
#define KP_YAW 0.01
#define KI_YAW 0
#define KD_YAW 1

// Depth Controller //
#define KP_DEPTH 0
#define KI_DEPTH 0
#define KD_DEPTH 0

// Saturation Constants //
#define YAW_SAT 1		// upper limit of yaw controller
#define DEPTH_SAT 1		// upper limit of depth controller
#define INT_SAT 10		// upper limit of integral windup
#define DINT_SAT 10		// upper limit of depth integral windup

// Filter Values //
#define A1 0.3			// for MS5837 pressure sensor
#define A2 0.4			// for MS5837 pressure sensor

// Fluid Densities in kg/m^3 //
#define DENSITY_FRESHWATER 997
#define DENSITY_SALTWATER 1029

// Acceleration Due to Gravity in m/s^2 //
#define GRAVITY 9.81

// Depth Start Value //
#define DEPTH_START 50 // starting depth (mm)

// Depth Threshold Value //
#define DEPTH_STOP 10000	// threshold depth (mm)

// Temperature Threshold Value //
#define TEMP_STOP 50	// deg C

// Stop Timer //
#define STOP_TIME 4		// seconds

// SOS Leak Sensor Pin //
#define SOSPIN 27		// connected to GPIO 27

/******************************************************************************
 * Declare Threads
******************************************************************************/

void *navigation(void* arg);
void *depth_thread(void* arg);
void *safety_thread(void* arg);

/******************************************************************************
 * Global Variables
******************************************************************************/

// holds the setpoint data structure with current setpoints
setpoint_t setpoint;

// holds the system state structure with current system statesystem_state_t sstate;
system_state_t sstate;

// holds the calibration values for the MS5837 pressure sensor
pressure_calib_t pressure_calib;

// holds the latest pressure value from the MS5837 pressure sensor
ms5837_t ms5837;

// create structure for storing IMU data
bno055_t bno055;

// holds the latest temperature value from the DS18B20 temperature sensor
ds18b20_t ds18b20;

// holds the constants and latest errors of the yaw pid controller
pid_data_t yaw_pid;

// holds the constants and latest errors of the depth pid controller
pid_data_t depth_pid;

// motor channels
int motor_channels[] = {CHANNEL_1, CHANNEL_2, CHANNEL_3};

// Ignoring sstate
float depth = 0;

/******************************************************************************
* Main Function
******************************************************************************/

int main()
{
	// Initialize python interpreter
	Py_Initialize();

	//Set up Pi GPIO pins through wiringPi
	wiringPiSetupGpio();

	// Check if AUV is initialized correctly //
	if(scripps_auv_init()<0)
	{
		return -1;
	}
	printf("\nAll components are initializated\n");
	substate.mode = INITIALIZING;

	// Initialize threads
	sched_param param;
	int policy, maxpriority;

	// Initialize priorities
	pthread_attr_init(&tattrlow);
	pthread_attr_init(&tattrmed);
	pthread_attr_init(&tattrhigh);

	// Get max priority
	pthread_attr_getschedpolicy(&tattrlow, &policy);
	maxpriority = sched_get_priority_max(policy);

	// Extract scheduling parameter
	pthread_attr_getschedparam (&tattrlow, &param);

	// Set up low priority
	param.sched_priority = maxpriority/4;
	pthread_attr_setschedparam (&tattrlow, &param);

	// Set up medium priority
	param.sched_priority = maxpriority/2;
	pthread_attr_setschedparam (&tattrmed, &param);

	// Set up high priority
	param.sched_priority = maxpriority-1;
	pthread_attr_setschedparam (&tattrhigh, &param);

	// Thread handles
	pthread_t navigationThread;
	pthread_t depthThread;
	pthread_t safetyThread;

	// Create threads using modified attributes
	pthread_create (&navigationThread, &tattrmed, navigation, NULL);
//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
//	pthread_create (&safetyThread, &tattrlow, safety_thread, NULL);

//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);

	// Destroy the thread attributes
	pthread_attr_destroy(&tattrlow);
	pthread_attr_destroy(&tattrmed);
	pthread_attr_destroy(&tattrhigh);

	// Start timer!
	time_t start = time(0);

	// Run main while loop, wait until it's time to stop
	while(substate.mode != STOPPED)
	{
		// Check if we've passed the stop time
		if(difftime(time(0),start) > STOP_TIME)
			substate.mode = STOPPED;

		// Sleep a little
		usleep(100000);
	}

	// Exit cleanly
	cleanup_auv();
	return 0;
}

/******************************************************************************
* Depth Thread
*
* For Recording Depth & Determining If AUV is in Water or Not
******************************************************************************/
void *depth_thread(void* arg)
{
	// initialize pressure sensor //
	pressure_calib = init_ms5837();

	while(substate.mode!=STOPPED)
	{
		// read pressure sensor by passing calibration structure //
		ms5837 = ms5837_read(pressure_calib);

		// calculate depth (no idea what the magic numbers are)
		depth = (ms5837.pressure-1013)*10.197-88.8; // units?
		// 1013: ambient pressure (mbar)

		usleep(10000);
	}

	pthread_exit(NULL);
}

/******************************************************************************
 * Navigation Thread
 *
 * For yaw control
 *****************************************************************************/

void *navigation(void* arg)
{
	initialize_motors(motor_channels, HERTZ);

	//float output_port;		// port motor output
	//float output_starboard; // starboard motor output

	// initialize Motor Percent to be returned by yaw_controller //
	float motor_percent;

	// initialize old imu data //
	yaw_pid.old = 0;

	// initialize setpoint for yaw_controller //
	yaw_pid.setpoint = 0;

	// initialize error values to be used in yaw_controller //
	yaw_pid.err = 0;
	yaw_pid.i_err = 0;

	// yaw_controller constant initialization ///
	yaw_pid.kp = 0.01;
	yaw_pid.kd = 0;
	yaw_pid.ki = 0;

	// range-from-bottom setpoint //
	depth_pid.setpoint = 2;	// meters

	// depth controller constant initialization
	depth_pid.kp = 0.01;
	depth_pid.kd = 0;
	depth_pid.ki = 0;

	// hard set motor speed //
	// pwmWrite(PIN_BASE+motor_channels[1], output_starboard)
	//set_motor(0, -0.2);  // right
	//set_motor(1, 0.2); // left
    //set_motor(2, 0.0);

	while(substate.mode!=STOPPED)
	{
		// read IMU values
	bno055 = bno055_read();

    if (bno055.yaw < 180)
	{
	yaw_pid.err = abs(bno055.yaw - yaw_pid.setpoint);
	}
	else
	{
	yaw_pid.err =abs((bno055.yaw-360) - yaw_pid.setpoint);
	}
	// Write captured values to screen
    /*printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
				 bno055.yaw, bno055.pitch, bno055.roll,
				 bno055.p, bno055.q, bno055.r,
				 bno055.sys, bno055.gyro, bno055.accel,
				 bno055.mag);*/

	// Sanity test: Check if yaw control works

	//Call yaw controller function
	yaw_controller();

	//set port motor
	//set_motors(0,motor_percent);

	//set starboard motor
	//set_motors(1, motor_percent);

	printf("\nYawPID_err: %f Motor Percent: %f ", yaw_pid.err, motor_percent);
	// sleep for 5 ms //
		usleep(5000);
	}

	//set_motor(0, 0);
	//set_motor(1, 0);
    //set_motor(2, 0);

	pthread_exit(NULL);
}

/******************************************************************************
 * Safety Thread
 *
 * Shuts down AUV if vehicle goes belows 10m, temperature gets too high, or
 * water intrusion is detected
 *****************************************************************************/
void *safety_thread(void* arg)
{
	// set up WiringPi for use // (not sure if actually needed)
	wiringPiSetup();

	// leak detection variables //
	int leakStatePin = digitalRead(SOSPIN);	// read the input pin
	int i;									// loop counting integer

	// leak detection pins //
	pinMode(SOSPIN, INPUT);					// set SOSPIN as an INPUT
	pinMode(17, OUTPUT);					// set GPIO 17 as an OUTPUT
	digitalWrite(leakStatePin, HIGH);		// set GPIO 17 as HIGH (VCC)

	while( substate.mode!=STOPPED )
	{
		/******************************************************************************
		 * Depth Protection
		 *
		 * Shut down AUV if vehicle travels deeper than 10m
		 *****************************************************************************/

		// get pressure value //
		pressure_calib = init_ms5837();
		ms5837 = ms5837_read(pressure_calib);
		sstate.depth[0] = (ms5837.pressure-1013)*10.197-88.8;
		//sstate.depth[0] = (ms5837.pressure*UNITS_KPA-101.325)/
		//	(DENSITY_FRESHWATER*GRAVITY)*0.000001;		// current depth in mm
		printf("%f\n", sstate.depth[0]);

		// filtered depth value //
		sstate.fdepth[0] = A1*(sstate.depth[0]+sstate.depth[1])+A2*sstate.fdepth[1];
		printf("Current depth is %f\n m", sstate.fdepth[0]/1000);

		//if( sstate.fdepth[0]< DEPTH_STOP )
		if( sstate.depth[0] < DEPTH_STOP )
		{
			substate.mode = RUNNING;
		}
		else
		{
			substate.mode = STOPPED;
			printf("%s\n", "STOPPED");
		}

		// set current depth values as old values //
		sstate.depth[1] = sstate.depth[0];
		sstate.fdepth[1] = sstate.fdepth[0];

		// sleep for 50 ms //
		usleep(50000);

		/******************************************************************************
		 * Temperature Protection
		 *
		 * Shut down AUV if housing temperature exceeds 50 deg C
		 *****************************************************************************/

		// read temperature values from DS18B20 temperature sensor //
		ds18b20 = ds18b20_read(); 	// temperature in deg C

		// let 'em know how hot we are //
		printf("Temperature: %f", ds18b20.temperature);

		// turn motors off if housing temperature gets too high //
		if( ds18b20.temperature > TEMP_STOP )
		{
			for( i=0; i<3; i++ )
			{
				pwmWrite(PIN_BASE+i, MOTOR_0);
			}
		}

		/******************************************************************************
		 * Leak Protection
		 *
		 * Shut down AUV if a leak is detected
		 *****************************************************************************/

		// check leak sensor for water intrusion //
		if( leakStatePin == HIGH )
		{
			// tell 'em it's bad //
			printf("LEAK DETECTED!\n");
			for( i=0; i<3; i++ )
			{
				// shut off motors //
				set_motor(PIN_BASE+i, MOTOR_0);
			}
        }
		else if (leakStatePin == LOW)
		{
			// tell 'em we're dry //
			printf("We're dry.\n");
		}

		pthread_exit(NULL);
	}

    return NULL;
}

/******************************************************************************
 * Logging Thread
 *
 * Logs the sensor output data into a file
 *****************************************************************************/
/*
PI_THREAD (logging_thread)
{
	while(substate.mode!=STOPPED){
		FILE *fd = fopen("log.txt", "a");
		char buffer[100] = {0};
		// add logging values to the next line
		sprintf(buffer, "%f %f %f %f %i %i %i %i %f %f %f %f\n",sstate.roll, sstate.pitch[0], sstate.yaw[0], sstate.depth[0],sstate.x[0],
		sstate.y[0], sstate.radius[0], setpoint.x - sstate.x[0], sstate.esc_out[0], sstate.esc_out[1], sstate.esc_out[2], sstate.esc_out[3]);
		fputs(buffer, fd);
		fclose(fd);
		//sleep for 100 ms

		usleep(100000);
	}
	return 0;
}
*/
<|endoftext|>


<commit_before>/******************************************************************************
*	Main script for the 2017 RoboFishy Scripps AUV
******************************************************************************/

#include "Mapper.h"

// Multithreading
#include <pthread.h>
#include <sched.h>
#include <unistd.h>

// Sampling Values
#define SAMPLE_RATE 200 // sample rate of main control loop (Hz)
#define DT 0.005				// timestep; make sure this is equal to 1/SAMPLE_RATE!

// Conversion Factors
#define UNITS_KPA 0.1 // converts pressure from mbar to kPa

/******************************************************************************
* Controller Gains
******************************************************************************/

// Yaw Controller
#define KP_YAW 0.01
#define KI_YAW 0
#define KD_YAW 1

// Depth Controller
#define KP_DEPTH 0
#define KI_DEPTH 0
#define KD_DEPTH 0

// Saturation Constants
#define YAW_SAT 1			// upper limit of yaw controller
#define DEPTH_SAT 1		// upper limit of depth controller
#define INT_SAT 10		// upper limit of integral windup
#define DINT_SAT 10		// upper limit of depth integral windup

// Filter Values
#define A1 0.3			// for MS5837 pressure sensor
#define A2 0.4			// for MS5837 pressure sensor

// Fluid Densities in kg/m^3
#define DENSITY_FRESHWATER 997
#define DENSITY_SALTWATER 1029

// Acceleration Due to Gravity in m/s^2
#define GRAVITY 9.81

// Depth Start Value
#define DEPTH_START 50 // starting depth (mm)

// Depth Threshold Value
#define DEPTH_STOP 2000	// threshold depth (mm)

// Temperature Threshold Value
#define TEMP_STOP 25	// deg C

// Stop Timer
#define STOP_TIME 4		// seconds

// Leak Sensor Inpu and Power Pin
#define LEAKPIN 27		// connected to GPIO 27
#define LEAKPOWERPIN 17 // providing Vcc to leak board

/******************************************************************************
 * Declare Threads
******************************************************************************/

void *navigation(void* arg);
void *depth_thread(void* arg);
void *safety_thread(void* arg);

/******************************************************************************
 * Global Variables
******************************************************************************/

// Holds the setpoint data structure with current setpoints
//setpoint_t setpoint;

// Holds the system state structure with current system statesystem_state_t sstate;
//system_state_t sstate;

// Holds the calibration values for the MS5837 pressure sensor
pressure_calib_t pressure_calib;

// Holds the latest pressure value from the MS5837 pressure sensor
ms5837_t ms5837;

// Create structure for storing IMU data
//bno055_t bno055;

// Holds the latest temperature value from the DS18B20 temperature sensor
ds18b20_t ds18b20;

// Holds the constants and latest errors of the yaw pid controller
pid_data_t yaw_pid;

// Holds the constants and latest errors of the depth pid controller
pid_data_t depth_pid;

// Motor channels
int motor_channels[] = {CHANNEL_1, CHANNEL_2, CHANNEL_3};

// Ignoring sstate
float depth = 0;

//yaw_controller intialization
float motor_percent = 0;

/******************************************************************************
* Main Function
******************************************************************************/

int main()
{
	// Initialize Python interpreter
	Py_Initialize();

	// Set up RasPi GPIO pins through wiringPi
	wiringPiSetupGpio();

	// Check if AUV is initialized correctly
	if( initialize_sensors() < 0 )
	{
		return -1;
	}
	printf("\nAll components are initialized\n");
	substate.mode = INITIALIZING;
    substate.laserarmed = ARMED;
	initializeTAttr();

	// Initialize threads
	/*sched_param param;
	int policy, maxpriority;

	// Initialize priorities
	pthread_attr_init(&tattrlow);
	pthread_attr_init(&tattrmed);
	pthread_attr_init(&tattrhigh);

	// Get max priority
	pthread_attr_getschedpolicy(&tattrlow, &policy);
	maxpriority = sched_get_priority_max(policy);

	// Extract scheduling parameter
	pthread_attr_getschedparam (&tattrlow, &param);

	// Set up low priority
	param.sched_priority = maxpriority/4;
	pthread_attr_setschedparam (&tattrlow, &param);

	// Set up medium priority
	param.sched_priority = maxpriority/2;
	pthread_attr_setschedparam (&tattrmed, &param);

	// Set up high priority
	param.sched_priority = maxpriority-1;
	pthread_attr_setschedparam (&tattrhigh, &param);*/

	// Thread handles
	//pthread_t navigationThread;
	//pthread_t depthThread;
	pthread_t safetyThread;
	//pthread_t disarmlaserThread;

	// Create threads using modified attributes
//	pthread_create (&navigationThread, &tattrmed, navigation, NULL);
//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
	pthread_create (&safetyThread, &tattrlow, safety_thread, NULL);

//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
//	pthread_create (&disarmlaserThread, &tattrlow, disarmLaser, NULL);
 	destroyTAttr();
	// Destroy the thread attributes
	/*pthread_attr_destroy(&tattrlow);
	pthread_attr_destroy(&tattrmed);
	pthread_attr_destroy(&tattrhigh);*/

	// Start timer!
	time_t start = time(0);

	// Run main while loop, wait until it's time to stop
	while(substate.mode != STOPPED)
	{
		// Check if we've passed the stop time
		if(difftime(time(0),start) > STOP_TIME)
			substate.mode = STOPPED;

		// Sleep a little
		usleep(100000);
	}

	// Exit cleanly
	cleanup_auv();
	return 0;
}

/******************************************************************************
* Depth Thread
*
* For Recording Depth & Determining If AUV is in Water or Not
******************************************************************************/
/*void *depth_thread(void* arg)
{
	// Initialize pressure sensor
	pressure_calib = init_pressure_sensor();

	while(substate.mode!=STOPPED)
	{
		// Read pressure sensor by passing calibration structure
		ms5837 = ms5837_read(pressure_calib);

		// Calculate depth (no idea what the magic numbers are)
		depth = (ms5837.pressure-1013)*10.197-88.8; // units?
		// 1013: ambient pressure (mbar)
		// 10.197*p_mbar = p_mmH20

		usleep(10000);
	}

	pthread_exit(NULL);
}*/

/******************************************************************************
 * Navigation Thread
 *
 * For yaw control
 *****************************************************************************/
/*
void *navigation(void* arg)
{
	initialize_motors(motor_channels, HERTZ);

	//float output_port;		// port motor output
	//float output_starboard; // starboard motor output

	// Initialize motor percent to be returned by yaw_controller
	//float motor_percent;

	// Initialize old imu data
	yaw_pid.old = 0;

	// Initialize setpoint for yaw_controller
	yaw_pid.setpoint = 0;

	// Initialize error values to be used in yaw_controller
	yaw_pid.err = 0;
	yaw_pid.i_err = 0;

	// Yaw controller constant initialization
	yaw_pid.kp = 0.01;
	yaw_pid.kd = 1;
	yaw_pid.ki = .1;

	// Range-from-bottom setpoint
	depth_pid.setpoint = 2;	// meters

	// Depth controller constant initialization
	depth_pid.kp = 0.001;
	depth_pid.kd = 0;
	depth_pid.ki = 0;

	// Initialize error values to be used in depth_controller
	depth_pid.err = 0;
	depth_pid.i_err = 0;

	// Hard set motor speed
	// pwmWrite(PIN_BASE+motor_channels[1], output_starboard)
	//set_motor(0, -0.2);  // right
	//set_motor(1, 0.2); // left
    //set_motor(2, 0.0);

	while(substate.mode!=STOPPED)
	{
		// read IMU values from fifo file
		//bno055 = read_imu_fifo();
		substate.imuorientation = read_imu_fifo();

	    if (substate.imuorientation.yaw < 180) //AUV pointed right
		{
			yaw_pid.err = substate.imuorientation.yaw - yaw_pid.setpoint;
		}
		else //AUV pointed left
		{
			yaw_pid.err =(substate.imuorientation.yaw-360) - yaw_pid.setpoint;
		}

		// Write captured values to screen
	    //printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
			 bno055.yaw, bno055.pitch, bno055.roll,
			 bno055.p, bno055.q, bno055.r,
			 bno055.sys, bno055.gyro, bno055.accel,
			 bno055.mag);*/

		// Sanity test: Check if yaw control works

		//Call yaw controller function
/*		marchPID(substate.imuorientation, yaw_pid);

		// Set port motor
		//set_motor(0,motor_percent);

		// Set starboard motor
		//set_motor(1, motor_percent);

		// Sleep for 5 ms //
	    if (substate.imuorientation.yaw < 180)
		{
		yaw_pid.err = abs(substate.imuorientation.yaw - yaw_pid.setpoint);
		}
		else
		{
		yaw_pid.err =abs((substate.imuorientation.yaw - 360) - yaw_pid.setpoint);
		}

		// Write captured values to screen
	    //printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
					 bno055.yaw, bno055.pitch, bno055.roll,
					 bno055.p, bno055.q, bno055.r,
					 bno055.sys, bno055.gyro, bno055.accel,
					 bno055.mag);*/

		//printf("\nYawPID_err: %f Motor Percent: %f ", yaw_pid.err, motor_percent);

		// Sanity test: Check if yaw control works
		/*
		// Call yaw controller function
		yaw_controller();

		// Set port motor
		set_motor(0,motor_percent);

		// Set starboard motor
		set_motor(1, motor_percent);

		*/

		// Sleep for 5 ms //
//		usleep(5000);
//	}

	//set_motor(0, 0);
	//set_motor(1, 0);
    //set_motor(2, 0);

//	pthread_exit(NULL);
//}

/******************************************************************************
 * Safety Thread
 *
 * Shuts down AUV if vehicle goes belows 10m, temperature gets too high, or
 * water intrusion is detected
 *****************************************************************************/
void *safety_thread(void* arg)
{
	// Set up WiringPi for use // (not sure if actually needed)
	wiringPiSetup();

	// Leak detection pins
	pinMode(LEAKPIN, INPUT);					// set LEAKPIN as an INPUT
	pinMode(LEAKPOWERPIN, OUTPUT);		// set as output to provide Vcc
	digitalWrite(LEAKPOWERPIN, HIGH);	// write high to provide Vcc

	// Leak checking variables
	int leakState;	// holds the state (HIGH or LOW) of the LEAKPIN

	// Test if temp sensor reads anything
	ds18b20 = read_temp_fifo();
	printf("Temperature: %f degC\n", ds18b20.temperature);

	/*while( substate.mode != STOPPED )
	{
		// Check if depth threshold has been exceeded
		if( substate.fdepth > DEPTH_STOP )
		{
			substate.mode = STOPPED;
			printf("We're too deep! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check temperature
		// Shut down AUV if housing temperature gets too high

		if( ds18b20.temperature > TEMP_STOP )
		{
			substate.mode = STOPPED;
			printf("It's too hot! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check for leak
		leakState = digitalRead(LEAKPIN);	// check the state of LEAKPIN
		if( leakState == HIGH )
		{
			substate.mode = STOPPED;
			printf("LEAK DETECTED! Shutting down...\n");
			continue;
		}
		else if (leakState == LOW)
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check IMU accelerometer for collision (1+ g detected)
		if( (float)fabs(substate.imuorientation.x_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.y_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.z_acc) > 1.0*GRAVITY )
		{
			substate.mode = STOPPED;
			printf("Collision detected. Shutting down...");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

	}*/

    pthread_exit(NULL);
}

/******************************************************************************
 * Logging Thread
 *
 * Logs the sensor output data into a file
 *****************************************************************************/
/*
PI_THREAD (logging_thread)
{
	while(substate.mode!=STOPPED){
		FILE *fd = fopen("log.txt", "a");
		char buffer[100] = {0};
		// add logging values to the next line
		sprintf(buffer, "%f %f %f %f %i %i %i %i %f %f %f %f\n",sstate.roll, sstate.pitch[0], sstate.yaw[0], sstate.depth[0],sstate.x[0],
		sstate.y[0], sstate.radius[0], setpoint.x - sstate.x[0], sstate.esc_out[0], sstate.esc_out[1], sstate.esc_out[2], sstate.esc_out[3]);
		fputs(buffer, fd);
		fclose(fd);
		//sleep for 100 ms

		usleep(100000);
	}
	return 0;
}
*/
<commit_msg>removed BNO055.py and BNO055.pyc<commit_after>/******************************************************************************
*	Main script for the 2017 RoboFishy Scripps AUV
******************************************************************************/

#include "Mapper.h"

// Multithreading
#include <pthread.h>
#include <sched.h>
#include <unistd.h>

// Sampling Values
#define SAMPLE_RATE 200 // sample rate of main control loop (Hz)
#define DT 0.005				// timestep; make sure this is equal to 1/SAMPLE_RATE!

// Conversion Factors
#define UNITS_KPA 0.1 // converts pressure from mbar to kPa

/******************************************************************************
* Controller Gains
******************************************************************************/

// Yaw Controller
#define KP_YAW 0.01
#define KI_YAW 0
#define KD_YAW 1

// Depth Controller
#define KP_DEPTH 0
#define KI_DEPTH 0
#define KD_DEPTH 0

// Saturation Constants
#define YAW_SAT 1			// upper limit of yaw controller
#define DEPTH_SAT 1		// upper limit of depth controller
#define INT_SAT 10		// upper limit of integral windup
#define DINT_SAT 10		// upper limit of depth integral windup

// Filter Values
#define A1 0.3			// for MS5837 pressure sensor
#define A2 0.4			// for MS5837 pressure sensor

// Fluid Densities in kg/m^3
#define DENSITY_FRESHWATER 997
#define DENSITY_SALTWATER 1029

// Acceleration Due to Gravity in m/s^2
#define GRAVITY 9.81

// Depth Start Value
#define DEPTH_START 50 // starting depth (mm)

// Depth Threshold Value
#define DEPTH_STOP 2000	// threshold depth (mm)

// Temperature Threshold Value
#define TEMP_STOP 25	// deg C

// Stop Timer
#define STOP_TIME 4		// seconds

// Leak Sensor Inpu and Power Pin
#define LEAKPIN 27		// connected to GPIO 27
#define LEAKPOWERPIN 17 // providing Vcc to leak board

/******************************************************************************
 * Declare Threads
******************************************************************************/

void *navigation(void* arg);
void *depth_thread(void* arg);
void *safety_thread(void* arg);

/******************************************************************************
 * Global Variables
******************************************************************************/

// Holds the setpoint data structure with current setpoints
//setpoint_t setpoint;

// Holds the system state structure with current system statesystem_state_t sstate;
//system_state_t sstate;

// Holds the calibration values for the MS5837 pressure sensor
pressure_calib_t pressure_calib;

// Holds the latest pressure value from the MS5837 pressure sensor
ms5837_t ms5837;

// Create structure for storing IMU data
//bno055_t bno055;

// Holds the latest temperature value from the DS18B20 temperature sensor
ds18b20_t ds18b20;

// Holds the constants and latest errors of the yaw pid controller
pid_data_t yaw_pid;

// Holds the constants and latest errors of the depth pid controller
pid_data_t depth_pid;

// Motor channels
int motor_channels[] = {CHANNEL_1, CHANNEL_2, CHANNEL_3};

// Ignoring sstate
float depth = 0;

//yaw_controller intialization
float motor_percent = 0;

/******************************************************************************
* Main Function
******************************************************************************/

int main()
{
	// Initialize Python interpreter
	Py_Initialize();

	// Set up RasPi GPIO pins through wiringPi
	wiringPiSetupGpio();

	// Check if AUV is initialized correctly
	if( initialize_sensors() < 0 )
	{
		return -1;
	}
	printf("\nAll components are initialized\n");
	substate.mode = INITIALIZING;
    substate.laserarmed = ARMED;
	initializeTAttr();

	// Initialize threads
	/*sched_param param;
	int policy, maxpriority;

	// Initialize priorities
	pthread_attr_init(&tattrlow);
	pthread_attr_init(&tattrmed);
	pthread_attr_init(&tattrhigh);

	// Get max priority
	pthread_attr_getschedpolicy(&tattrlow, &policy);
	maxpriority = sched_get_priority_max(policy);

	// Extract scheduling parameter
	pthread_attr_getschedparam (&tattrlow, &param);

	// Set up low priority
	param.sched_priority = maxpriority/4;
	pthread_attr_setschedparam (&tattrlow, &param);

	// Set up medium priority
	param.sched_priority = maxpriority/2;
	pthread_attr_setschedparam (&tattrmed, &param);

	// Set up high priority
	param.sched_priority = maxpriority-1;
	pthread_attr_setschedparam (&tattrhigh, &param);*/

	// Thread handles
	//pthread_t navigationThread;
	//pthread_t depthThread;
	pthread_t safetyThread;
	//pthread_t disarmlaserThread;

	// Create threads using modified attributes
//	pthread_create (&navigationThread, &tattrmed, navigation, NULL);
//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
	pthread_create (&safetyThread, &tattrlow, safety_thread, NULL);

//	pthread_create (&depthThread, &tattrmed, depth_thread, NULL);
//	pthread_create (&disarmlaserThread, &tattrlow, disarmLaser, NULL);
 	destroyTAttr();
	// Destroy the thread attributes
	/*pthread_attr_destroy(&tattrlow);
	pthread_attr_destroy(&tattrmed);
	pthread_attr_destroy(&tattrhigh);*/

	// Start timer!
	time_t start = time(0);

	// Run main while loop, wait until it's time to stop
	while(substate.mode != STOPPED)
	{
		// Check if we've passed the stop time
		if(difftime(time(0),start) > STOP_TIME)
			substate.mode = STOPPED;

		// Sleep a little
		usleep(100000);
	}

	// Exit cleanly
	cleanup_auv();
	return 0;
}

/******************************************************************************
* Depth Thread
*
* For Recording Depth & Determining If AUV is in Water or Not
******************************************************************************/
/*void *depth_thread(void* arg)
{
	// Initialize pressure sensor
	pressure_calib = init_pressure_sensor();

	while(substate.mode!=STOPPED)
	{
		// Read pressure sensor by passing calibration structure
		ms5837 = ms5837_read(pressure_calib);

		// Calculate depth (no idea what the magic numbers are)
		depth = (ms5837.pressure-1013)*10.197-88.8; // units?
		// 1013: ambient pressure (mbar)
		// 10.197*p_mbar = p_mmH20

		usleep(10000);
	}

	pthread_exit(NULL);
}*/

/******************************************************************************
 * Navigation Thread
 *
 * For yaw control
 *****************************************************************************/
/*
void *navigation(void* arg)
{
	initialize_motors(motor_channels, HERTZ);

	//float output_port;		// port motor output
	//float output_starboard; // starboard motor output

	// Initialize motor percent to be returned by yaw_controller
	//float motor_percent;

	// Initialize old imu data
	yaw_pid.old = 0;

	// Initialize setpoint for yaw_controller
	yaw_pid.setpoint = 0;

	// Initialize error values to be used in yaw_controller
	yaw_pid.err = 0;
	yaw_pid.i_err = 0;

	// Yaw controller constant initialization
	yaw_pid.kp = 0.01;
	yaw_pid.kd = 1;
	yaw_pid.ki = .1;

	// Range-from-bottom setpoint
	depth_pid.setpoint = 2;	// meters

	// Depth controller constant initialization
	depth_pid.kp = 0.001;
	depth_pid.kd = 0;
	depth_pid.ki = 0;

	// Initialize error values to be used in depth_controller
	depth_pid.err = 0;
	depth_pid.i_err = 0;

	// Hard set motor speed
	// pwmWrite(PIN_BASE+motor_channels[1], output_starboard)
	//set_motor(0, -0.2);  // right
	//set_motor(1, 0.2); // left
    //set_motor(2, 0.0);

	while(substate.mode!=STOPPED)
	{
		// read IMU values from fifo file
		//bno055 = read_imu_fifo();
		substate.imuorientation = read_imu_fifo();

	    if (substate.imuorientation.yaw < 180) //AUV pointed right
		{
			yaw_pid.err = substate.imuorientation.yaw - yaw_pid.setpoint;
		}
		else //AUV pointed left
		{
			yaw_pid.err =(substate.imuorientation.yaw-360) - yaw_pid.setpoint;
		}

		// Write captured values to screen
	    //printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
			 bno055.yaw, bno055.pitch, bno055.roll,
			 bno055.p, bno055.q, bno055.r,
			 bno055.sys, bno055.gyro, bno055.accel,
			 bno055.mag);*/

		// Sanity test: Check if yaw control works

		//Call yaw controller function
/*		marchPID(substate.imuorientation, yaw_pid);

		// Set port motor
		//set_motor(0,motor_percent);

		// Set starboard motor
		//set_motor(1, motor_percent);

		// Sleep for 5 ms //
	    if (substate.imuorientation.yaw < 180)
		{
		yaw_pid.err = abs(substate.imuorientation.yaw - yaw_pid.setpoint);
		}
		else
		{
		yaw_pid.err =abs((substate.imuorientation.yaw - 360) - yaw_pid.setpoint);
		}

		// Write captured values to screen
	    //printf("\nYaw: %f Roll: %f Pitch: %f p: %f q: %f r: %f Sys: %i Gyro: %i Accel: %i Mag: %i\n ",
					 bno055.yaw, bno055.pitch, bno055.roll,
					 bno055.p, bno055.q, bno055.r,
					 bno055.sys, bno055.gyro, bno055.accel,
					 bno055.mag);*/

		//printf("\nYawPID_err: %f Motor Percent: %f ", yaw_pid.err, motor_percent);

		// Sanity test: Check if yaw control works
		/*
		// Call yaw controller function
		yaw_controller();

		// Set port motor
		set_motor(0,motor_percent);

		// Set starboard motor
		set_motor(1, motor_percent);

		*/

		// Sleep for 5 ms //
//		usleep(5000);
//	}

	//set_motor(0, 0);
	//set_motor(1, 0);
    //set_motor(2, 0);

//	pthread_exit(NULL);
//}

/******************************************************************************
 * Safety Thread
 *
 * Shuts down AUV if vehicle goes belows 10m, temperature gets too high, or
 * water intrusion is detected
 *****************************************************************************/
void *safety_thread(void* arg)
{
	// Set up WiringPi for use // (not sure if actually needed)
	wiringPiSetup();

	// Leak detection pins
	pinMode(LEAKPIN, INPUT);					// set LEAKPIN as an INPUT
	pinMode(LEAKPOWERPIN, OUTPUT);		// set as output to provide Vcc
	digitalWrite(LEAKPOWERPIN, HIGH);	// write high to provide Vcc

	// Leak checking variables
	int leakState;	// holds the state (HIGH or LOW) of the LEAKPIN

	// Test if temp sensor reads anything
	ds18b20 = read_temp_fifo();
	printf("Temperature: %f degC\n", ds18b20.temperature);

	while( substate.mode != STOPPED )
	{
		// Check if depth threshold has been exceeded
		/*if( substate.fdepth > DEPTH_STOP )
		{
			substate.mode = STOPPED;
			printf("We're too deep! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}*/

		// Check temperature
		// Shut down AUV if housing temperature gets too high

		if( ds18b20.temperature > TEMP_STOP )
		{
			substate.mode = STOPPED;
			printf("It's too hot! Shutting down...\n");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check for leak
		/*leakState = digitalRead(LEAKPIN);	// check the state of LEAKPIN
		if( leakState == HIGH )
		{
			substate.mode = STOPPED;
			printf("LEAK DETECTED! Shutting down...\n");
			continue;
		}
		else if (leakState == LOW)
		{
			// We're still good
			substate.mode = RUNNING;
		}

		// Check IMU accelerometer for collision (1+ g detected)
		if( (float)fabs(substate.imuorientation.x_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.y_acc) > 1.0*GRAVITY
			|| (float)fabs(substate.imuorientation.z_acc) > 1.0*GRAVITY )
		{
			substate.mode = STOPPED;
			printf("Collision detected. Shutting down...");
			continue;
		}
		else
		{
			// We're still good
			substate.mode = RUNNING;
		}*/

    pthread_exit(NULL);
}

/******************************************************************************
 * Logging Thread
 *
 * Logs the sensor output data into a file
 *****************************************************************************/
/*
PI_THREAD (logging_thread)
{
	while(substate.mode!=STOPPED){
		FILE *fd = fopen("log.txt", "a");
		char buffer[100] = {0};
		// add logging values to the next line
		sprintf(buffer, "%f %f %f %f %i %i %i %i %f %f %f %f\n",sstate.roll, sstate.pitch[0], sstate.yaw[0], sstate.depth[0],sstate.x[0],
		sstate.y[0], sstate.radius[0], setpoint.x - sstate.x[0], sstate.esc_out[0], sstate.esc_out[1], sstate.esc_out[2], sstate.esc_out[3]);
		fputs(buffer, fd);
		fclose(fd);
		//sleep for 100 ms

		usleep(100000);
	}
	return 0;
}
*/
<|endoftext|>


<commit_before>#include "dsa_common.h"

#include "certificate.h"

#include <boost/filesystem.hpp>

#include <cmath>
#include <memory>
using std::unique_ptr;
#include <random>
#include <string>

#if defined(_MSC_VER)
#include <openssl/applink.c>
#endif

#include <openssl/bio.h>
#include <openssl/bn.h>
#include <openssl/pem.h>
#include <openssl/rsa.h>
#include <openssl/x509v3.h>

using BIO_FILE_ptr = std::unique_ptr<BIO, decltype(&::BIO_free)>;
using BN_ptr = std::unique_ptr<BIGNUM, decltype(&::BN_free)>;
using EVP_KEY_ptr = std::unique_ptr<EVP_PKEY, decltype(&::EVP_PKEY_free)>;
using RSA_ptr = std::unique_ptr<RSA, decltype(&::RSA_free)>;
using X509_ptr = std::unique_ptr<X509, decltype(&::X509_free)>;

#include "module/logger.h"

namespace dsa {

void generate_certificate() {
  int rc;

  RSA_ptr rsa(RSA_new(), ::RSA_free);
  BN_ptr bn(BN_new(), ::BN_free);

  if (rc = BN_set_word(bn.get(), RSA_F4) != 1) {
    LOG_FATAL(LOG << "BN_set_word failed!"
                  << " rc=" << rc);
  }

  if (rc = RSA_generate_key_ex(rsa.get(), 4098, bn.get(), NULL) != 1) {
    LOG_FATAL(LOG << "RSA_generate_key_ex failed!"
                  << " rc=" << rc);
  }

  EVP_KEY_ptr pkey(EVP_PKEY_new(), ::EVP_PKEY_free);
  if (rc = EVP_PKEY_set1_RSA(pkey.get(), rsa.get()) != 1) {
    LOG_FATAL(LOG << "EVP_PKEY_set1_RSA failed!"
                  << " rc=" << rc);
  }

#if 0
    // write public key
    BIO_FILE_ptr pubkey(BIO_new_file("pubkey.pem", "w"), ::BIO_free);
    if (rc = PEM_write_bio_PUBKEY(pubkey.get(), pkey.get()) != 1) {
      LOG_FATAL(LOG << "PEM_write_bio_PUBKEY failed!" << " rc=" << rc);
    }
#endif

  // write private key
  std::string privkey_fname("key.pem");
  BIO_FILE_ptr privkey(BIO_new_file(privkey_fname.c_str(), "w"), ::BIO_free);
  if (rc = PEM_write_bio_RSAPrivateKey(privkey.get(), rsa.get(), NULL, NULL, 0,
                                       NULL, NULL) != 1) {
    LOG_FATAL(LOG << "PEM_write_bio_RSAPrivate failed!"
                  << " rc=" << rc);
  }

  namespace fs = boost::filesystem;
  fs::permissions(privkey_fname.c_str(), fs::owner_read|fs::owner_write);

  // generate certificate
  X509_ptr x509(X509_new(), ::X509_free);

  X509_set_version(x509.get(), 2L);

  if (rc = X509_set_pubkey(x509.get(), pkey.get()) != 1) {
    LOG_FATAL(LOG << "X509_set_pubkey failed!"
                  << " rc=" << rc);
  }

  std::random_device rd;
  std::mt19937_64 e2(rd());
  std::uniform_int_distribution<int64_t> dist(std::llround(std::pow(2, 61)),
                                              std::llround(std::pow(2, 62)));

  ASN1_INTEGER_set(X509_get_serialNumber(x509.get()), dist(e2));

  X509_gmtime_adj((ASN1_TIME *)X509_get_notBefore(x509.get()), 0);
  X509_gmtime_adj((ASN1_TIME *)X509_get_notAfter(x509.get()),
                  31536000000ull);  // 365000 days

  X509_NAME *name;
  name = X509_get_subject_name(x509.get());
  X509_NAME_add_entry_by_txt(name, "C", MBSTRING_ASC, (unsigned char *)"US", -1,
                             -1, 0);
  X509_NAME_add_entry_by_txt(name, "ST", MBSTRING_ASC,
                             (unsigned char *)"California", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "L", MBSTRING_ASC,
                             (unsigned char *)"Berkeley", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "O", MBSTRING_ASC,
                             (unsigned char *)"Acuity Brands, Inc.", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "OU", MBSTRING_ASC,
                             (unsigned char *)"Customers", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "CN", MBSTRING_ASC, (unsigned char *)"*", -1,
                             -1, 0);
  if (X509_set_issuer_name(x509.get(), name) != 1) {
    LOG_FATAL(LOG << "X509_set_issuer_name failed!");
  }

  if (!X509_sign(x509.get(), pkey.get(), EVP_sha256())) {
    LOG_FATAL(LOG << "X509_sign failed!");
  }

  FILE *f = fopen("certificate.pem", "wb");
  if (!PEM_write_X509(f, x509.get())) {
    fclose(f);
    LOG_FATAL(LOG << "PEM_write_X509 failed!");
  }
  fclose(f);

  return;
}

}  // namespace dsa
<commit_msg>fix certificate error on 32bit openssl<commit_after>#include "dsa_common.h"

#include "certificate.h"

#include <boost/filesystem.hpp>

#include <cmath>
#include <memory>
using std::unique_ptr;
#include <random>
#include <string>

#if defined(_MSC_VER)
#include <openssl/applink.c>
#endif

#include <openssl/bio.h>
#include <openssl/bn.h>
#include <openssl/pem.h>
#include <openssl/rsa.h>
#include <openssl/x509v3.h>

using BIO_FILE_ptr = std::unique_ptr<BIO, decltype(&::BIO_free)>;
using BN_ptr = std::unique_ptr<BIGNUM, decltype(&::BN_free)>;
using EVP_KEY_ptr = std::unique_ptr<EVP_PKEY, decltype(&::EVP_PKEY_free)>;
using RSA_ptr = std::unique_ptr<RSA, decltype(&::RSA_free)>;
using X509_ptr = std::unique_ptr<X509, decltype(&::X509_free)>;

#include "module/logger.h"

namespace dsa {

void generate_certificate() {
  int rc;

  RSA_ptr rsa(RSA_new(), ::RSA_free);
  BN_ptr bn(BN_new(), ::BN_free);

  if (rc = BN_set_word(bn.get(), RSA_F4) != 1) {
    LOG_FATAL(LOG << "BN_set_word failed!"
                  << " rc=" << rc);
  }

  if (rc = RSA_generate_key_ex(rsa.get(), 4098, bn.get(), NULL) != 1) {
    LOG_FATAL(LOG << "RSA_generate_key_ex failed!"
                  << " rc=" << rc);
  }

  EVP_KEY_ptr pkey(EVP_PKEY_new(), ::EVP_PKEY_free);
  if (rc = EVP_PKEY_set1_RSA(pkey.get(), rsa.get()) != 1) {
    LOG_FATAL(LOG << "EVP_PKEY_set1_RSA failed!"
                  << " rc=" << rc);
  }

#if 0
    // write public key
    BIO_FILE_ptr pubkey(BIO_new_file("pubkey.pem", "w"), ::BIO_free);
    if (rc = PEM_write_bio_PUBKEY(pubkey.get(), pkey.get()) != 1) {
      LOG_FATAL(LOG << "PEM_write_bio_PUBKEY failed!" << " rc=" << rc);
    }
#endif

  // write private key
  std::string privkey_fname("key.pem");
  BIO_FILE_ptr privkey(BIO_new_file(privkey_fname.c_str(), "w"), ::BIO_free);
  if (rc = PEM_write_bio_RSAPrivateKey(privkey.get(), rsa.get(), NULL, NULL, 0,
                                       NULL, NULL) != 1) {
    LOG_FATAL(LOG << "PEM_write_bio_RSAPrivate failed!"
                  << " rc=" << rc);
  }

  namespace fs = boost::filesystem;
  fs::permissions(privkey_fname.c_str(), fs::owner_read|fs::owner_write);

  // generate certificate
  X509_ptr x509(X509_new(), ::X509_free);

  X509_set_version(x509.get(), 2L);

  if (rc = X509_set_pubkey(x509.get(), pkey.get()) != 1) {
    LOG_FATAL(LOG << "X509_set_pubkey failed!"
                  << " rc=" << rc);
  }

  std::random_device rd;
  std::mt19937_64 e2(rd());
  std::uniform_int_distribution<int64_t> dist(std::llround(std::pow(2, 61)),
                                              std::llround(std::pow(2, 62)));

  ASN1_INTEGER_set(X509_get_serialNumber(x509.get()), dist(e2));

  X509_gmtime_adj((ASN1_TIME *)X509_get_notBefore(x509.get()), 0);
  X509_gmtime_adj((ASN1_TIME *)X509_get_notAfter(x509.get()),
                  0x7fffffff);

  X509_NAME *name;
  name = X509_get_subject_name(x509.get());
  X509_NAME_add_entry_by_txt(name, "C", MBSTRING_ASC, (unsigned char *)"US", -1,
                             -1, 0);
  X509_NAME_add_entry_by_txt(name, "ST", MBSTRING_ASC,
                             (unsigned char *)"California", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "L", MBSTRING_ASC,
                             (unsigned char *)"Berkeley", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "O", MBSTRING_ASC,
                             (unsigned char *)"Acuity Brands, Inc.", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "OU", MBSTRING_ASC,
                             (unsigned char *)"Customers", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "CN", MBSTRING_ASC, (unsigned char *)"*", -1,
                             -1, 0);
  if (X509_set_issuer_name(x509.get(), name) != 1) {
    LOG_FATAL(LOG << "X509_set_issuer_name failed!");
  }

  if (!X509_sign(x509.get(), pkey.get(), EVP_sha256())) {
    LOG_FATAL(LOG << "X509_sign failed!");
  }

  FILE *f = fopen("certificate.pem", "wb");
  if (!PEM_write_X509(f, x509.get())) {
    fclose(f);
    LOG_FATAL(LOG << "PEM_write_X509 failed!");
  }
  fclose(f);

  return;
}

}  // namespace dsa
<|endoftext|>


<commit_before>#include "dsa_common.h"

#include "certificate.h"

#include <boost/filesystem.hpp>

#include <cmath>
#include <memory>
using std::unique_ptr;
#include <random>
#include <string>

#include <openssl/bio.h>
#include <openssl/bn.h>
#include <openssl/pem.h>
#include <openssl/rsa.h>
#include <openssl/x509v3.h>

using BIO_FILE_ptr = std::unique_ptr<BIO, decltype(&::BIO_free)>;
using BN_ptr = std::unique_ptr<BIGNUM, decltype(&::BN_free)>;
using EVP_KEY_ptr = std::unique_ptr<EVP_PKEY, decltype(&::EVP_PKEY_free)>;
using RSA_ptr = std::unique_ptr<RSA, decltype(&::RSA_free)>;
using X509_ptr = std::unique_ptr<X509, decltype(&::X509_free)>;

#include "module/logger.h"

namespace dsa {

void generate_certificate() {
  int rc;

  RSA_ptr rsa(RSA_new(), ::RSA_free);
  BN_ptr bn(BN_new(), ::BN_free);

  if (rc = BN_set_word(bn.get(), RSA_F4) != 1) {
    LOG_FATAL(LOG << "BN_set_word failed!"
                  << " rc=" << rc);
  }

  if (rc = RSA_generate_key_ex(rsa.get(), 4098, bn.get(), NULL) != 1) {
    LOG_FATAL(LOG << "RSA_generate_key_ex failed!"
                  << " rc=" << rc);
  }

  EVP_KEY_ptr pkey(EVP_PKEY_new(), ::EVP_PKEY_free);
  if (rc = EVP_PKEY_set1_RSA(pkey.get(), rsa.get()) != 1) {
    LOG_FATAL(LOG << "EVP_PKEY_set1_RSA failed!"
                  << " rc=" << rc);
  }

#if 0
    // write public key
    BIO_FILE_ptr pubkey(BIO_new_file("pubkey.pem", "w"), ::BIO_free);
    if (rc = PEM_write_bio_PUBKEY(pubkey.get(), pkey.get()) != 1) {
      LOG_FATAL(LOG << "PEM_write_bio_PUBKEY failed!" << " rc=" << rc);
    }
#endif

  // write private key
  std::string privkey_fname("key.pem");
  BIO_FILE_ptr privkey(BIO_new_file(privkey_fname.c_str(), "w"), ::BIO_free);
  if (rc = PEM_write_bio_RSAPrivateKey(privkey.get(), rsa.get(), NULL, NULL, 0,
                                       NULL, NULL) != 1) {
    LOG_FATAL(LOG << "PEM_write_bio_RSAPrivate failed!"
                  << " rc=" << rc);
  }

  namespace fs = boost::filesystem;
  fs::permissions(privkey_fname.c_str(), fs::owner_read|fs::owner_write);

  // generate certificate
  X509_ptr x509(X509_new(), ::X509_free);

  X509_set_version(x509.get(), 2L);

  if (rc = X509_set_pubkey(x509.get(), pkey.get()) != 1) {
    LOG_FATAL(LOG << "X509_set_pubkey failed!"
                  << " rc=" << rc);
  }

  std::random_device rd;
  std::mt19937_64 e2(rd());
  std::uniform_int_distribution<int64_t> dist(std::llround(std::pow(2, 61)),
                                              std::llround(std::pow(2, 62)));

  ASN1_INTEGER_set(X509_get_serialNumber(x509.get()), dist(e2));

  X509_gmtime_adj((ASN1_TIME *)X509_get_notBefore(x509.get()), 0);
  X509_gmtime_adj((ASN1_TIME *)X509_get_notAfter(x509.get()),
                  31536000000ull);  // 365000 days

  X509_NAME *name;
  name = X509_get_subject_name(x509.get());
  X509_NAME_add_entry_by_txt(name, "C", MBSTRING_ASC, (unsigned char *)"US", -1,
                             -1, 0);
  X509_NAME_add_entry_by_txt(name, "ST", MBSTRING_ASC,
                             (unsigned char *)"California", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "L", MBSTRING_ASC,
                             (unsigned char *)"Berkeley", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "O", MBSTRING_ASC,
                             (unsigned char *)"Acuity Brands, Inc.", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "OU", MBSTRING_ASC,
                             (unsigned char *)"Customers", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "CN", MBSTRING_ASC, (unsigned char *)"*", -1,
                             -1, 0);
  if (X509_set_issuer_name(x509.get(), name) != 1) {
    LOG_FATAL(LOG << "X509_set_issuer_name failed!");
  }

  if (!X509_sign(x509.get(), pkey.get(), EVP_sha256())) {
    LOG_FATAL(LOG << "X509_sign failed!");
  }

  FILE *f = fopen("certificate.pem", "wb");
  if (!PEM_write_X509(f, x509.get())) {
    fclose(f);
    LOG_FATAL(LOG << "PEM_write_X509 failed!");
  }
  fclose(f);

  return;
}

}  // namespace dsa
<commit_msg>fix 'no OPENSSL_Applink' issue on Windows platform<commit_after>#include "dsa_common.h"

#include "certificate.h"

#include <boost/filesystem.hpp>

#include <cmath>
#include <memory>
using std::unique_ptr;
#include <random>
#include <string>

#if defined(_MSC_VER)
#include <openssl/applink.c>
#endif

#include <openssl/bio.h>
#include <openssl/bn.h>
#include <openssl/pem.h>
#include <openssl/rsa.h>
#include <openssl/x509v3.h>

using BIO_FILE_ptr = std::unique_ptr<BIO, decltype(&::BIO_free)>;
using BN_ptr = std::unique_ptr<BIGNUM, decltype(&::BN_free)>;
using EVP_KEY_ptr = std::unique_ptr<EVP_PKEY, decltype(&::EVP_PKEY_free)>;
using RSA_ptr = std::unique_ptr<RSA, decltype(&::RSA_free)>;
using X509_ptr = std::unique_ptr<X509, decltype(&::X509_free)>;

#include "module/logger.h"

namespace dsa {

void generate_certificate() {
  int rc;

  RSA_ptr rsa(RSA_new(), ::RSA_free);
  BN_ptr bn(BN_new(), ::BN_free);

  if (rc = BN_set_word(bn.get(), RSA_F4) != 1) {
    LOG_FATAL(LOG << "BN_set_word failed!"
                  << " rc=" << rc);
  }

  if (rc = RSA_generate_key_ex(rsa.get(), 4098, bn.get(), NULL) != 1) {
    LOG_FATAL(LOG << "RSA_generate_key_ex failed!"
                  << " rc=" << rc);
  }

  EVP_KEY_ptr pkey(EVP_PKEY_new(), ::EVP_PKEY_free);
  if (rc = EVP_PKEY_set1_RSA(pkey.get(), rsa.get()) != 1) {
    LOG_FATAL(LOG << "EVP_PKEY_set1_RSA failed!"
                  << " rc=" << rc);
  }

#if 0
    // write public key
    BIO_FILE_ptr pubkey(BIO_new_file("pubkey.pem", "w"), ::BIO_free);
    if (rc = PEM_write_bio_PUBKEY(pubkey.get(), pkey.get()) != 1) {
      LOG_FATAL(LOG << "PEM_write_bio_PUBKEY failed!" << " rc=" << rc);
    }
#endif

  // write private key
  std::string privkey_fname("key.pem");
  BIO_FILE_ptr privkey(BIO_new_file(privkey_fname.c_str(), "w"), ::BIO_free);
  if (rc = PEM_write_bio_RSAPrivateKey(privkey.get(), rsa.get(), NULL, NULL, 0,
                                       NULL, NULL) != 1) {
    LOG_FATAL(LOG << "PEM_write_bio_RSAPrivate failed!"
                  << " rc=" << rc);
  }

  namespace fs = boost::filesystem;
  fs::permissions(privkey_fname.c_str(), fs::owner_read|fs::owner_write);

  // generate certificate
  X509_ptr x509(X509_new(), ::X509_free);

  X509_set_version(x509.get(), 2L);

  if (rc = X509_set_pubkey(x509.get(), pkey.get()) != 1) {
    LOG_FATAL(LOG << "X509_set_pubkey failed!"
                  << " rc=" << rc);
  }

  std::random_device rd;
  std::mt19937_64 e2(rd());
  std::uniform_int_distribution<int64_t> dist(std::llround(std::pow(2, 61)),
                                              std::llround(std::pow(2, 62)));

  ASN1_INTEGER_set(X509_get_serialNumber(x509.get()), dist(e2));

  X509_gmtime_adj((ASN1_TIME *)X509_get_notBefore(x509.get()), 0);
  X509_gmtime_adj((ASN1_TIME *)X509_get_notAfter(x509.get()),
                  31536000000ull);  // 365000 days

  X509_NAME *name;
  name = X509_get_subject_name(x509.get());
  X509_NAME_add_entry_by_txt(name, "C", MBSTRING_ASC, (unsigned char *)"US", -1,
                             -1, 0);
  X509_NAME_add_entry_by_txt(name, "ST", MBSTRING_ASC,
                             (unsigned char *)"California", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "L", MBSTRING_ASC,
                             (unsigned char *)"Berkeley", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "O", MBSTRING_ASC,
                             (unsigned char *)"Acuity Brands, Inc.", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "OU", MBSTRING_ASC,
                             (unsigned char *)"Customers", -1, -1, 0);
  X509_NAME_add_entry_by_txt(name, "CN", MBSTRING_ASC, (unsigned char *)"*", -1,
                             -1, 0);
  if (X509_set_issuer_name(x509.get(), name) != 1) {
    LOG_FATAL(LOG << "X509_set_issuer_name failed!");
  }

  if (!X509_sign(x509.get(), pkey.get(), EVP_sha256())) {
    LOG_FATAL(LOG << "X509_sign failed!");
  }

  FILE *f = fopen("certificate.pem", "wb");
  if (!PEM_write_X509(f, x509.get())) {
    fclose(f);
    LOG_FATAL(LOG << "PEM_write_X509 failed!");
  }
  fclose(f);

  return;
}

}  // namespace dsa
<|endoftext|>