<commit_before>/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2012 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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.
*/

/// \author James Hughes
/// \date   September 2012
/// \brief  Not sure this file should go in Modules/Render. But it is an
///         auxiliary file to the ViewScene render module.

#include <QMouseEvent>
#include <QWheelEvent>
#include <QTimer>
#include <Core/Application/Application.h>

#include "GLWidget.h"

using namespace SCIRun::Gui;
using Spire::Vector2;

//------------------------------------------------------------------------------
GLWidget::GLWidget(QtGLContext* context) :
    QGLWidget(context),
    mContext(new GLContext(this))
{
  /// \todo Implement this intelligently. This function is called everytime
  ///       there is a new graphics context.
  std::vector<std::string> shaderSearchDirs;

  auto shadersInBinDirectory = SCIRun::Core::Application::Instance().executablePath() / "Shaders";
  shaderSearchDirs.push_back(shadersInBinDirectory.string());

#ifdef SPIRE_USE_STD_THREADS
  mGraphics = std::shared_ptr<Spire::SCIRun::SRInterface>(
      new Spire::SCIRun::SRInterface(
          std::dynamic_pointer_cast<Spire::Context>(mContext),
          shaderSearchDirs, true));
#else
  mGraphics = std::shared_ptr<Spire::SCIRun::SRInterface>(
      new Spire::SCIRun::SRInterface(
          std::dynamic_pointer_cast<Spire::Context>(mContext),
          shaderSearchDirs, false));
  mTimer = new QTimer(this);
  connect(mTimer, SIGNAL(timeout()), this, SLOT(updateRenderer()));
  mTimer->start(35);
#endif

  /// \todo Where should we store common shader names?
  mGraphics->getStuPipe()->addPersistentShader(
      "UniformColor",
      { {"UniformColor.vs", Spire::StuInterface::VERTEX_SHADER},
        {"UniformColor.fs", Spire::StuInterface::FRAGMENT_SHADER},
      });

  // We must disable auto buffer swap on the 'paintEvent'.
  setAutoBufferSwap(false);
}

//------------------------------------------------------------------------------
GLWidget::~GLWidget()
{
  // Need to inform module that the context is being destroyed.
  if (mGraphics != nullptr)
  {
    std::cout << "Terminating spire." << std::endl;
    mGraphics->terminate();
    mGraphics.reset();
  }
}

//------------------------------------------------------------------------------
void GLWidget::initializeGL()
{
}

//------------------------------------------------------------------------------
void GLWidget::mouseMoveEvent(QMouseEvent* event)
{
  /// \todo Include specific button info.
  mGraphics->inputMouseMove(Vector2<int32_t>(event->x(), event->y()));
}

//------------------------------------------------------------------------------
void GLWidget::mousePressEvent(QMouseEvent* event)
{
  /// \todo Include specific button info.
  mGraphics->inputMouseDown(Vector2<int32_t>(event->x(), event->y()));
}

//------------------------------------------------------------------------------
void GLWidget::mouseReleaseEvent(QMouseEvent* event)
{
  /// \todo Include specific button info.
  mGraphics->inputMouseUp(Vector2<int32_t>(event->x(), event->y()));
}

//------------------------------------------------------------------------------
void GLWidget::wheelEvent(QWheelEvent * event)
{
  mGraphics->inputMouseWheel(event->delta());
}

//------------------------------------------------------------------------------
void GLWidget::resizeGL(int width, int height)
{
  mGraphics->eventResize(static_cast<int32_t>(width),
                         static_cast<int32_t>(height));
}

//------------------------------------------------------------------------------
void GLWidget::updateRenderer()
{
  mContext->makeCurrent();
  mGraphics->doFrame();
  mContext->swapBuffers();
}

<commit_msg>Removing initializer lists for MSVC.<commit_after>/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2012 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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.
*/

/// \author James Hughes
/// \date   September 2012
/// \brief  Not sure this file should go in Modules/Render. But it is an
///         auxiliary file to the ViewScene render module.

#include <QMouseEvent>
#include <QWheelEvent>
#include <QTimer>
#include <Core/Application/Application.h>

#include "GLWidget.h"

using namespace SCIRun::Gui;
using Spire::Vector2;

//------------------------------------------------------------------------------
GLWidget::GLWidget(QtGLContext* context) :
    QGLWidget(context),
    mContext(new GLContext(this))
{
  /// \todo Implement this intelligently. This function is called everytime
  ///       there is a new graphics context.
  std::vector<std::string> shaderSearchDirs;

  auto shadersInBinDirectory = SCIRun::Core::Application::Instance().executablePath() / "Shaders";
  shaderSearchDirs.push_back(shadersInBinDirectory.string());

#ifdef SPIRE_USE_STD_THREADS
  mGraphics = std::shared_ptr<Spire::SCIRun::SRInterface>(
      new Spire::SCIRun::SRInterface(
          std::dynamic_pointer_cast<Spire::Context>(mContext),
          shaderSearchDirs, true));
#else
  mGraphics = std::shared_ptr<Spire::SCIRun::SRInterface>(
      new Spire::SCIRun::SRInterface(
          std::dynamic_pointer_cast<Spire::Context>(mContext),
          shaderSearchDirs, false));
  mTimer = new QTimer(this);
  connect(mTimer, SIGNAL(timeout()), this, SLOT(updateRenderer()));
  mTimer->start(35);
#endif

  /// \todo Where should we store common shader names?
  std::vector<std::tuple<std::string, Spire::StuInterface::SHADER_TYPES>> shaderFiles;
  shaderFiles.push_back(std::make_pair("UniformColor.vs", Spire::StuInterface::VERTEX_SHADER));
  shaderFiles.push_back(std::make_pair("UniformColor.fs", Spire::StuInterface::FRAGMENT_SHADER));

  mGraphics->getStuPipe()->addPersistentShader(
      "UniformColor",
      shaderFiles);
//      { {"UniformColor.vs", Spire::StuInterface::VERTEX_SHADER},
//        {"UniformColor.fs", Spire::StuInterface::FRAGMENT_SHADER},
//      });

  // We must disable auto buffer swap on the 'paintEvent'.
  setAutoBufferSwap(false);
}

//------------------------------------------------------------------------------
GLWidget::~GLWidget()
{
  // Need to inform module that the context is being destroyed.
  if (mGraphics != nullptr)
  {
    std::cout << "Terminating spire." << std::endl;
    mGraphics->terminate();
    mGraphics.reset();
  }
}

//------------------------------------------------------------------------------
void GLWidget::initializeGL()
{
}

//------------------------------------------------------------------------------
void GLWidget::mouseMoveEvent(QMouseEvent* event)
{
  /// \todo Include specific button info.
  mGraphics->inputMouseMove(Vector2<int32_t>(event->x(), event->y()));
}

//------------------------------------------------------------------------------
void GLWidget::mousePressEvent(QMouseEvent* event)
{
  /// \todo Include specific button info.
  mGraphics->inputMouseDown(Vector2<int32_t>(event->x(), event->y()));
}

//------------------------------------------------------------------------------
void GLWidget::mouseReleaseEvent(QMouseEvent* event)
{
  /// \todo Include specific button info.
  mGraphics->inputMouseUp(Vector2<int32_t>(event->x(), event->y()));
}

//------------------------------------------------------------------------------
void GLWidget::wheelEvent(QWheelEvent * event)
{
  mGraphics->inputMouseWheel(event->delta());
}

//------------------------------------------------------------------------------
void GLWidget::resizeGL(int width, int height)
{
  mGraphics->eventResize(static_cast<int32_t>(width),
                         static_cast<int32_t>(height));
}

//------------------------------------------------------------------------------
void GLWidget::updateRenderer()
{
  mContext->makeCurrent();
  mGraphics->doFrame();
  mContext->swapBuffers();
}

<|endoftext|>


<commit_before>/*
 * =====================================================================================
 *
 *       Filename:  TGM_Tangram.cpp
 *
 *    Description:
 *
 *        Created:  08/22/2012 08:38:37 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Jiantao Wu (),
 *   Inistitution:  Boston College
 *
 * =====================================================================================
 */

#include <cstdio>
#include <cstdlib>

#include "TGM_Array.h"
#include "TGM_Parameters.h"
#include "TGM_FragLenTable.h"
#include "TGM_LibTable.h"
#include "TGM_BamPair.h"
#include "TGM_Detector.h"
#include "TGM_Reference.h"
#include "TGM_Aligner.h"
#include "TGM_Printer.h"

#include "api/BamMultiReader.h"

using namespace std;
using namespace Tangram;
using namespace BamTools;

int main(int argc, char *argv[])
{
    DetectPars detectPars;
    AlignerPars alignerPars;
    Parameters parameters(detectPars, alignerPars);
    parameters.Set((const char**) argv, argc);

    vector<string> filenames;
    parameters.SetBamFilenames(filenames);

    BamMultiReader bamMultiReader;
    bamMultiReader.Open(filenames);
    bamMultiReader.LocateIndexes();

    LibTable libTable;
    libTable.Read(detectPars.fpLibInput, detectPars.maxFragDiff);

    FragLenTable fragLenTable;
    fragLenTable.Read(detectPars.fpHistInput);

    parameters.ParseRangeStr(bamMultiReader);
    parameters.SetRange(bamMultiReader, libTable.GetFragLenMax());

    BamPairTable bamPairTable(libTable, fragLenTable, detectPars.minSoftSize, detectPars.minMQ, detectPars.spMinMQ);

    BamAlignment alignment;
    while(bamMultiReader.GetNextAlignment(alignment))
    {
        bamPairTable.Update(alignment);
    }

    fragLenTable.Destory();

    Detector detector(detectPars, libTable, bamPairTable);
    detector.Init();

    detector.CallEvents();

    const Reference* pRef = NULL;
    const Aligner* pAligner = NULL;

    Reference reference;
    Aligner aligner(detector, bamPairTable,  alignerPars, reference, libTable);

    if (alignerPars.fpRefInput != NULL)
    {
        // always load the whole chromosome reference
        reference.Read(alignerPars.fpRefInput, detectPars.refID, -1, -1);
        reference.CreateFamilyToZA(*(libTable.GetSpecialRefNames()));

        aligner.Map();

        pRef = &reference;
        pAligner = &aligner;
    }

    Printer printer(&detector, detectPars, pAligner, pRef, libTable, bamPairTable);
    printer.Print();

    bamMultiReader.Close();
    return EXIT_SUCCESS;
}

<commit_msg>Change the process of locating the bam index files according to Derek's suggestion<commit_after>/*
 * =====================================================================================
 *
 *       Filename:  TGM_Tangram.cpp
 *
 *    Description:
 *
 *        Created:  08/22/2012 08:38:37 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Jiantao Wu (),
 *   Inistitution:  Boston College
 *
 * =====================================================================================
 */

#include <cstdio>
#include <cstdlib>

#include "TGM_Array.h"
#include "TGM_Parameters.h"
#include "TGM_FragLenTable.h"
#include "TGM_LibTable.h"
#include "TGM_BamPair.h"
#include "TGM_Detector.h"
#include "TGM_Reference.h"
#include "TGM_Aligner.h"
#include "TGM_Printer.h"

#include "api/BamMultiReader.h"

using namespace std;
using namespace Tangram;
using namespace BamTools;

int main(int argc, char *argv[])
{
    // load the command line arguments
    DetectPars detectPars;
    AlignerPars alignerPars;
    Parameters parameters(detectPars, alignerPars);
    parameters.Set((const char**) argv, argc);

    // load the input bam file names
    vector<string> filenames;
    parameters.SetBamFilenames(filenames);

    // open the bam files with bam multi reader
    BamMultiReader bamMultiReader;
    bamMultiReader.Open(filenames);

    // make sure we have the index files at with the input bam files
    if (!bamMultiReader.LocateIndexes())
    {
        // fail to locate the index files
        // we need to create them if necessary
        if (!bamMultiReader.CreateIndexes())
            TGM_ErrQuit("ERROR: Cannot locate or create the index files for the input bam files.\n");
    }

    // read the library information table
    LibTable libTable;
    libTable.Read(detectPars.fpLibInput, detectPars.maxFragDiff);

    // read the fragment length distribution table
    FragLenTable fragLenTable;
    fragLenTable.Read(detectPars.fpHistInput);

    // where should we start to call the SV events
    parameters.ParseRangeStr(bamMultiReader);
    parameters.SetRange(bamMultiReader, libTable.GetFragLenMax());

    BamPairTable bamPairTable(libTable, fragLenTable, detectPars.minSoftSize, detectPars.minMQ, detectPars.spMinMQ);

    // iterate through the bam files and fill the bam pair table
    BamAlignment alignment;
    while(bamMultiReader.GetNextAlignment(alignment))
    {
        bamPairTable.Update(alignment);
    }

    // clean the fragnment length table
    fragLenTable.Destory();

    // call the SV events with read-pair signal
    Detector detector(detectPars, libTable, bamPairTable);
    detector.Init();
    detector.CallEvents();

    const Reference* pRef = NULL;
    const Aligner* pAligner = NULL;

    Reference reference;
    Aligner aligner(detector, bamPairTable,  alignerPars, reference, libTable);

    // call the SV events with split-read signal
    if (alignerPars.fpRefInput != NULL)
    {
        // always load the whole chromosome reference
        reference.Read(alignerPars.fpRefInput, detectPars.refID, -1, -1);
        reference.CreateFamilyToZA(*(libTable.GetSpecialRefNames()));

        aligner.Map();

        pRef = &reference;
        pAligner = &aligner;
    }

    // print out the events (vcf format)
    Printer printer(&detector, detectPars, pAligner, pRef, libTable, bamPairTable);
    printer.Print();

    // clean up and quit
    bamMultiReader.Close();
    return EXIT_SUCCESS;
}

<|endoftext|>


<commit_before>/*=========================================================================

Library:   IntersonArray

Copyright Kitware Inc. 28 Corporate Drive,
Clifton Park, NY, 12065, USA.

All rights reserved.

Licensed under the Apache License, Version 2.0 ( the "License" );
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

=========================================================================*/
#pragma unmanaged
#include "IntersonArrayCxxImagingContainer.h"
#include <iostream>
#pragma managed

#include <vcclr.h>
#include <msclr/marshal_cppstd.h>
#include <msclr/auto_gcroot.h>

#using "IntersonArray.dll"

namespace IntersonArrayCxx
{

namespace Imaging
{

//
// Begin Capture
//
ref class NewRFImageHandler
{
public:

  typedef Container::NewRFImageCallbackType   NewRFImageCallbackType;

  typedef IntersonArrayCxx::Imaging::Container::RFPixelType        RFPixelType;
  typedef IntersonArrayCxx::Imaging::Container::RFImagePixelType   RFImagePixelType;
  typedef cli::array< RFPixelType, 2 >                             RFArrayType;

  NewRFImageHandler( RFArrayType ^managedRFBuffer ):
    NewRFImageCallback( NULL ),
    NewRFImageCallbackClientData( NULL ),
    bufferWidth( Container::MAX_RFSAMPLES ),
    bufferHeight( Container::NBOFLINES ),
    NativeRFBuffer(new RFImagePixelType[Container::MAX_RFSAMPLES
      * Container::NBOFLINES] ),
    ManagedRFBuffer( managedRFBuffer )
  {
  }

  ~NewRFImageHandler()
  {
    delete [] NativeRFBuffer;
  }

  void HandleNewRFImage( IntersonArray::Imaging::Capture ^scan2D,
    System::EventArgs ^eventArgs )
  {
    if ( this->NewRFImageCallback != NULL )
    {
      for ( int ii = 0; ii < bufferHeight; ++ii )
      {
        for ( int jj = 0; jj < bufferWidth; ++jj )
        {
          this->NativeRFBuffer[bufferWidth * ii + jj] = (short)
            this->ManagedRFBuffer[ii, jj];
        }
      }
      this->NewRFImageCallback( this->NativeRFBuffer,
        this->NewRFImageCallbackClientData );
    }
  }

  void SetImageSize( int width, int height )
  {
     //bufferWidth = width;
     //bufferHeight = height;
  }

  void SetNewRFImageCallback( NewRFImageCallbackType callback,
    void *clientData )
  {
    this->NewRFImageCallback = callback;
    this->NewRFImageCallbackClientData = clientData;
  }

private:
  NewRFImageCallbackType   NewRFImageCallback;
  void                   * NewRFImageCallbackClientData;
  RFImagePixelType       * NativeRFBuffer;
  RFArrayType            ^ ManagedRFBuffer;
  int                      bufferWidth;
  int                      bufferHeight;
};

ref class NewImageHandler
{
public:
  typedef Container::NewImageCallbackType        NewImageCallbackType;

  typedef IntersonArrayCxx::Imaging::Container::PixelType     PixelType;
  typedef cli::array< byte, 2 >                               ArrayType;

  NewImageHandler( ArrayType ^managedBuffer ):
    NewImageCallback( NULL ),
    NewImageCallbackClientData( NULL ),
    bufferWidth( Container::MAX_SAMPLES ),
    bufferHeight( Container::NBOFLINES ),
    NativeBuffer( new PixelType[Container::MAX_SAMPLES *
      Container::NBOFLINES ] ),
    ManagedBuffer( managedBuffer )
  {
     std::cout << "Conatiner NBOFLINES: " << Container::NBOFLINES << std::endl;

  }

  ~NewImageHandler()
  {
    delete [] NativeBuffer;
  }

  void HandleNewImage( IntersonArray::Imaging::Capture ^scan2D,
    System::EventArgs ^eventArgs )
  {
    if ( this->NewImageCallback != NULL )
    {
      for ( int ii = 0; ii < bufferHeight; ++ii )
      {
        for ( int jj = 0; jj < bufferWidth; ++jj )
        {
          this->NativeBuffer[bufferWidth * ii + jj] =
            this->ManagedBuffer[ii, jj];
        }
      }
      this->NewImageCallback( this->NativeBuffer,
        this->NewImageCallbackClientData );
    }
  }

  void SetImageSize( int width, int height )
  {
    // bufferWidth = width;
    // bufferHeight = height;
  }

  void SetNewImageCallback( NewImageCallbackType callback,
    void *clientData )
  {
    this->NewImageCallback = callback;
    this->NewImageCallbackClientData = clientData;
  }

private:
  NewImageCallbackType   NewImageCallback;
  void                 * NewImageCallbackClientData;
  PixelType            * NativeBuffer;
  ArrayType            ^ ManagedBuffer;
  int                    bufferWidth;
  int                    bufferHeight;
};

// End Capture

class ContainerImpl
{
public:
  typedef cli::array< Container::PixelType, 2 >      ArrayType;
  typedef cli::array< Container::RFPixelType, 2 >    RFArrayType;

  typedef Container::NewImageCallbackType      NewImageCallbackType;
  typedef Container::NewRFImageCallbackType    NewRFImageCallbackType;

  ContainerImpl()
  {
    WrappedScanConverter = gcnew IntersonArray::Imaging::ScanConverter();
    WrappedImageBuilding = gcnew IntersonArray::Imaging::ImageBuilding();
    WrappedCapture = gcnew IntersonArray::Imaging::Capture();

    Buffer = gcnew ArrayType( Container::NBOFLINES, Container::MAX_SAMPLES );
    Handler = gcnew NewImageHandler( Buffer );
    HandlerDelegate = gcnew
      IntersonArray::Imaging::Capture::NewImageHandler( Handler,
        & NewImageHandler::HandleNewImage );
    WrappedCapture->NewImageTick += HandlerDelegate;

    RFBuffer = gcnew RFArrayType(Container::NBOFLINES , Container::MAX_RFSAMPLES);
    RFHandler = gcnew NewRFImageHandler( RFBuffer );
    RFHandlerDelegate = gcnew
      IntersonArray::Imaging::Capture::NewImageHandler( RFHandler,
        & NewRFImageHandler::HandleNewRFImage );
  }

  ~ContainerImpl()
  {
    WrappedCapture->NewImageTick -= RFHandlerDelegate;
    WrappedCapture->NewImageTick -= HandlerDelegate;
  }

  bool GetCompound()
  {
    return WrappedScanConverter->Compound;
  }

  void SetCompound( bool value )
  {
    WrappedScanConverter->Compound = value;
  }

  bool GetDoubler()
  {
    return WrappedScanConverter->Doubler;
  }

  void SetDoubler( bool value )
  {
    WrappedScanConverter->Doubler = value;
  }

  int GetHeightScan() const
  {
    return WrappedScanConverter->HeightScan;
  }

  float GetMmPerPixel() const
  {
    return WrappedScanConverter->MmPerPixel;
  }

  double GetTrueDepth() const
  {
    return WrappedScanConverter->TrueDepth;
  }

  int GetWidthScan() const
  {
    return WrappedScanConverter->WidthScan;
  }

  int GetZeroOfYScale() const
  {
    return WrappedScanConverter->ZeroOfYScale;
  }

  Container::ScanConverterError HardInitScanConverter( int depth,
    int widthScan, int heightScan, int steering )
  {

    return static_cast< Container::ScanConverterError >(
      WrappedScanConverter->HardInitScanConverter( depth, widthScan,
        heightScan, steering, WrappedCapture.get(),
        WrappedImageBuilding.get() ) );
  }

  Container::ScanConverterError IdleInitScanConverter( int depth,
    int widthScan, int heightScan, short idleId, int idleSteering,
    bool idleDoubler, bool idleCompound, int idleCompoundAngle )
  {
    return static_cast< Container::ScanConverterError >(
      WrappedScanConverter->IdleInitScanConverter( depth, widthScan,
        heightScan, idleId, idleSteering, idleDoubler, idleCompound,
        idleCompoundAngle, WrappedImageBuilding.get() ) );
  }

  //
  // Begin Capture
  //
  bool GetFrameAvg()
  {
    return WrappedCapture->FrameAvg;
  }

  void SetFrameAvg( bool value)
  {
    WrappedCapture->FrameAvg = value;
  }

  bool GetRFData()
  {
    return WrappedCapture->RFData;
  }

  void SetRFData( bool value )
  {
    if (value){
      this->hwControls->ReadFPGAVersion();
    }
    WrappedCapture->RFData = value;
    //Set correct image handler
    if (value){
      WrappedCapture->NewImageTick -= HandlerDelegate;
      WrappedCapture->NewImageTick += RFHandlerDelegate;
    }
    else{
      WrappedCapture->NewImageTick -= RFHandlerDelegate;
      WrappedCapture->NewImageTick += HandlerDelegate;
    }
  }

  double GetScanOn()
  {
    return WrappedCapture->ScanOn;
  }

  void AbortScan()
  {
    WrappedCapture->AbortScan();
  }

  void DisposeScan()
  {
    WrappedCapture->DisposeScan();
  }

  void StartReadScan()
  {
    WrappedCapture->StartReadScan( (ArrayType ^)Buffer );
  }

  void StartRFReadScan()
  {
    WrappedCapture->StartRFReadScan( (RFArrayType ^)RFBuffer );
  }

  void StopReadScan()
  {
    WrappedCapture->StopReadScan();
  }

  void SetNewImageCallback( NewImageCallbackType callback,
    void *clientData = 0 )
  {
    this->Handler->SetNewImageCallback( callback, clientData );

    //this->Handler->SetImageSize( this->GetWidthScan(),
    //  this->GetHeightScan() );
  }

  void SetNewRFImageCallback( NewRFImageCallbackType callback,
    void *clientData = 0 )
  {
    this->RFHandler->SetNewRFImageCallback( callback, clientData );
    this->RFHandler->SetImageSize(Container::MAX_RFSAMPLES, Container::NBOFLINES);
  }
  void SetHWControls(IntersonArrayCxx::Controls::HWControls * controls)
  {
    this->hwControls = controls;
  }

  //
  // Begin Wrapped ImageBuilding
  //

private:
  IntersonArrayCxx::Controls::HWControls * hwControls;
  msclr::auto_gcroot< IntersonArray::Imaging::ScanConverter ^ >
    WrappedScanConverter;
  msclr::auto_gcroot< IntersonArray::Imaging::ImageBuilding ^ >
    WrappedImageBuilding;
  msclr::auto_gcroot< IntersonArray::Imaging::Capture ^ >
    WrappedCapture;

  gcroot< ArrayType ^ >                         Buffer;
  gcroot< RFArrayType ^ >                       RFBuffer;
  gcroot< NewImageHandler ^ >                   Handler;
  gcroot< NewRFImageHandler ^ >                 RFHandler;

  gcroot< IntersonArray::Imaging::Capture::NewImageHandler ^ >
    HandlerDelegate;
  gcroot< IntersonArray::Imaging::Capture::NewImageHandler ^ >
    RFHandlerDelegate;

};

#pragma unmanaged

Container
::Container():
  Impl( new ContainerImpl() )
{
}

Container
::~Container()
{
  delete Impl;
}

bool
Container
::GetCompound()
{
  return Impl->GetCompound();
}

void
Container
::SetCompound( bool value )
{
  Impl->SetCompound( value );
}

bool
Container
::GetDoubler()
{
  return Impl->GetDoubler();
}

void
Container
::SetDoubler( bool value )
{
  Impl->SetDoubler( value );
}

int
Container
::GetHeightScan() const
{
  return Impl->GetHeightScan();
}

float
Container
::GetMmPerPixel() const
{
  return Impl->GetMmPerPixel();
}

double
Container
::GetTrueDepth() const
{
  return Impl->GetMmPerPixel();
}

int
Container
::GetWidthScan() const
{
  return Impl->GetWidthScan();
}

int
Container
::GetZeroOfYScale() const
{
  return Impl->GetZeroOfYScale();
}

Container::ScanConverterError
Container
::HardInitScanConverter( int depth, int widthScan, int heightScan,
  int steering )
{
  return Impl->HardInitScanConverter( depth, widthScan, heightScan,
    steering );
}

Container::ScanConverterError
Container
::IdleInitScanConverter( int depth, int width, int height, short idleId,
  int idleSteering, bool idleDoubler, bool idleCompound,
  int idleCompoundAngle )
{
  return Impl->IdleInitScanConverter( depth, width, height, idleId,
    idleSteering, idleDoubler, idleCompound, idleCompoundAngle );
}

//
// Begin Capture
//
bool
Container
::GetRFData()
{
  return Impl->GetRFData();
}

void
Container
::SetRFData( bool transferOn )
{
  Impl->SetRFData( transferOn );
}

bool
Container
::GetFrameAvg()
{
  return Impl->GetFrameAvg();
}

void
Container
::SetFrameAvg( bool doAveraging )
{
  Impl->SetFrameAvg( doAveraging );
}

bool
Container
::GetScanOn() const
{
  return Impl->GetScanOn();
}

void
Container
::AbortScan()
{
  Impl->AbortScan();
}

void
Container
::DisposeScan()
{
  Impl->DisposeScan();
}

void
Container
::StartReadScan()
{
  Impl->StartReadScan();
}

void
Container
::StartRFReadScan()
{
  Impl->StartRFReadScan();
}

void
Container
::StopReadScan()
{
  Impl->StopReadScan();
}

void
Container
::SetNewImageCallback( NewImageCallbackType callback,
  void *clientData )
{
  Impl->SetNewImageCallback( callback, clientData );
}

void
Container
::SetNewRFImageCallback( NewRFImageCallbackType callback,
  void *clientData )
{
  Impl->SetNewRFImageCallback( callback, clientData );
}

void
Container
::SetHWControls(IntersonArrayCxx::Controls::HWControls * controls)
{
  Impl->SetHWControls(controls);
}

} // end namespace Imaging

} // end namespace IntersonArrayCxx
<commit_msg>Cleanup<commit_after>/*=========================================================================

Library:   IntersonArray

Copyright Kitware Inc. 28 Corporate Drive,
Clifton Park, NY, 12065, USA.

All rights reserved.

Licensed under the Apache License, Version 2.0 ( the "License" );
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

=========================================================================*/
#pragma unmanaged
#include "IntersonArrayCxxImagingContainer.h"
#include <iostream>
#pragma managed

#include <vcclr.h>
#include <msclr/marshal_cppstd.h>
#include <msclr/auto_gcroot.h>

#using "IntersonArray.dll"

namespace IntersonArrayCxx
{

namespace Imaging
{

//
// Begin Capture
//
ref class NewRFImageHandler
{
public:

  typedef Container::NewRFImageCallbackType   NewRFImageCallbackType;

  typedef IntersonArrayCxx::Imaging::Container::RFPixelType        RFPixelType;
  typedef IntersonArrayCxx::Imaging::Container::RFImagePixelType   RFImagePixelType;
  typedef cli::array< RFPixelType, 2 >                             RFArrayType;

  NewRFImageHandler( RFArrayType ^managedRFBuffer ):
    NewRFImageCallback( NULL ),
    NewRFImageCallbackClientData( NULL ),
    bufferWidth( Container::MAX_RFSAMPLES ),
    bufferHeight( Container::NBOFLINES ),
    NativeRFBuffer(new RFImagePixelType[Container::MAX_RFSAMPLES
      * Container::NBOFLINES] ),
    ManagedRFBuffer( managedRFBuffer )
  {
  }

  ~NewRFImageHandler()
  {
    delete [] NativeRFBuffer;
  }

  void HandleNewRFImage( IntersonArray::Imaging::Capture ^scan2D,
    System::EventArgs ^eventArgs )
  {
    if ( this->NewRFImageCallback != NULL )
    {
      for ( int ii = 0; ii < bufferHeight; ++ii )
      {
        for ( int jj = 0; jj < bufferWidth; ++jj )
        {
          this->NativeRFBuffer[bufferWidth * ii + jj] = (short)
            this->ManagedRFBuffer[ii, jj];
        }
      }
      this->NewRFImageCallback( this->NativeRFBuffer,
        this->NewRFImageCallbackClientData );
    }
  }

  void SetNewRFImageCallback( NewRFImageCallbackType callback,
    void *clientData )
  {
    this->NewRFImageCallback = callback;
    this->NewRFImageCallbackClientData = clientData;
  }

private:
  NewRFImageCallbackType   NewRFImageCallback;
  void                   * NewRFImageCallbackClientData;
  RFImagePixelType       * NativeRFBuffer;
  RFArrayType            ^ ManagedRFBuffer;
  int                      bufferWidth;
  int                      bufferHeight;
};

ref class NewImageHandler
{
public:
  typedef Container::NewImageCallbackType        NewImageCallbackType;

  typedef IntersonArrayCxx::Imaging::Container::PixelType     PixelType;
  typedef cli::array< byte, 2 >                               ArrayType;

  NewImageHandler( ArrayType ^managedBuffer ):
    NewImageCallback( NULL ),
    NewImageCallbackClientData( NULL ),
    bufferWidth( Container::MAX_SAMPLES ),
    bufferHeight( Container::NBOFLINES ),
    NativeBuffer( new PixelType[Container::MAX_SAMPLES *
      Container::NBOFLINES ] ),
    ManagedBuffer( managedBuffer )
  {
  }

  ~NewImageHandler()
  {
    delete [] NativeBuffer;
  }

  void HandleNewImage( IntersonArray::Imaging::Capture ^scan2D,
    System::EventArgs ^eventArgs )
  {
    if ( this->NewImageCallback != NULL )
    {
      for ( int ii = 0; ii < bufferHeight; ++ii )
      {
        for ( int jj = 0; jj < bufferWidth; ++jj )
        {
          this->NativeBuffer[bufferWidth * ii + jj] =
            this->ManagedBuffer[ii, jj];
        }
      }
      this->NewImageCallback( this->NativeBuffer,
        this->NewImageCallbackClientData );
    }
  }

  void SetNewImageCallback( NewImageCallbackType callback,
    void *clientData )
  {
    this->NewImageCallback = callback;
    this->NewImageCallbackClientData = clientData;
  }

private:
  NewImageCallbackType   NewImageCallback;
  void                 * NewImageCallbackClientData;
  PixelType            * NativeBuffer;
  ArrayType            ^ ManagedBuffer;
  int                    bufferWidth;
  int                    bufferHeight;
};

// End Capture

class ContainerImpl
{
public:
  typedef cli::array< Container::PixelType, 2 >      ArrayType;
  typedef cli::array< Container::RFPixelType, 2 >    RFArrayType;

  typedef Container::NewImageCallbackType      NewImageCallbackType;
  typedef Container::NewRFImageCallbackType    NewRFImageCallbackType;

  ContainerImpl()
  {
    WrappedScanConverter = gcnew IntersonArray::Imaging::ScanConverter();
    WrappedImageBuilding = gcnew IntersonArray::Imaging::ImageBuilding();
    WrappedCapture = gcnew IntersonArray::Imaging::Capture();

    Buffer = gcnew ArrayType( Container::NBOFLINES, Container::MAX_SAMPLES );
    Handler = gcnew NewImageHandler( Buffer );
    HandlerDelegate = gcnew
      IntersonArray::Imaging::Capture::NewImageHandler( Handler,
        & NewImageHandler::HandleNewImage );
    WrappedCapture->NewImageTick += HandlerDelegate;

    RFBuffer = gcnew RFArrayType(Container::NBOFLINES , Container::MAX_RFSAMPLES);
    RFHandler = gcnew NewRFImageHandler( RFBuffer );
    RFHandlerDelegate = gcnew
      IntersonArray::Imaging::Capture::NewImageHandler( RFHandler,
        & NewRFImageHandler::HandleNewRFImage );
  }

  ~ContainerImpl()
  {
    WrappedCapture->NewImageTick -= RFHandlerDelegate;
    WrappedCapture->NewImageTick -= HandlerDelegate;
  }

  bool GetCompound()
  {
    return WrappedScanConverter->Compound;
  }

  void SetCompound( bool value )
  {
    WrappedScanConverter->Compound = value;
  }

  bool GetDoubler()
  {
    return WrappedScanConverter->Doubler;
  }

  void SetDoubler( bool value )
  {
    WrappedScanConverter->Doubler = value;
  }

  int GetHeightScan() const
  {
    return WrappedScanConverter->HeightScan;
  }

  float GetMmPerPixel() const
  {
    return WrappedScanConverter->MmPerPixel;
  }

  double GetTrueDepth() const
  {
    return WrappedScanConverter->TrueDepth;
  }

  int GetWidthScan() const
  {
    return WrappedScanConverter->WidthScan;
  }

  int GetZeroOfYScale() const
  {
    return WrappedScanConverter->ZeroOfYScale;
  }

  Container::ScanConverterError HardInitScanConverter( int depth,
    int widthScan, int heightScan, int steering )
  {

    return static_cast< Container::ScanConverterError >(
      WrappedScanConverter->HardInitScanConverter( depth, widthScan,
        heightScan, steering, WrappedCapture.get(),
        WrappedImageBuilding.get() ) );
  }

  Container::ScanConverterError IdleInitScanConverter( int depth,
    int widthScan, int heightScan, short idleId, int idleSteering,
    bool idleDoubler, bool idleCompound, int idleCompoundAngle )
  {
    return static_cast< Container::ScanConverterError >(
      WrappedScanConverter->IdleInitScanConverter( depth, widthScan,
        heightScan, idleId, idleSteering, idleDoubler, idleCompound,
        idleCompoundAngle, WrappedImageBuilding.get() ) );
  }

  //
  // Begin Capture
  //
  bool GetFrameAvg()
  {
    return WrappedCapture->FrameAvg;
  }

  void SetFrameAvg( bool value)
  {
    WrappedCapture->FrameAvg = value;
  }

  bool GetRFData()
  {
    return WrappedCapture->RFData;
  }

  void SetRFData( bool value )
  {
    if (value){
      this->hwControls->ReadFPGAVersion();
    }
    WrappedCapture->RFData = value;
    //Set correct image handler
    if (value){
      WrappedCapture->NewImageTick -= HandlerDelegate;
      WrappedCapture->NewImageTick += RFHandlerDelegate;
    }
    else{
      WrappedCapture->NewImageTick -= RFHandlerDelegate;
      WrappedCapture->NewImageTick += HandlerDelegate;
    }
  }

  double GetScanOn()
  {
    return WrappedCapture->ScanOn;
  }

  void AbortScan()
  {
    WrappedCapture->AbortScan();
  }

  void DisposeScan()
  {
    WrappedCapture->DisposeScan();
  }

  void StartReadScan()
  {
    WrappedCapture->StartReadScan( (ArrayType ^)Buffer );
  }

  void StartRFReadScan()
  {
    WrappedCapture->StartRFReadScan( (RFArrayType ^)RFBuffer );
  }

  void StopReadScan()
  {
    WrappedCapture->StopReadScan();
  }

  void SetNewImageCallback( NewImageCallbackType callback,
    void *clientData = 0 )
  {
    this->Handler->SetNewImageCallback( callback, clientData );
  }

  void SetNewRFImageCallback( NewRFImageCallbackType callback,
    void *clientData = 0 )
  {
    this->RFHandler->SetNewRFImageCallback( callback, clientData );
  }
  void SetHWControls(IntersonArrayCxx::Controls::HWControls * controls)
  {
    this->hwControls = controls;
  }

  //
  // Begin Wrapped ImageBuilding
  //

private:
  IntersonArrayCxx::Controls::HWControls * hwControls;
  msclr::auto_gcroot< IntersonArray::Imaging::ScanConverter ^ >
    WrappedScanConverter;
  msclr::auto_gcroot< IntersonArray::Imaging::ImageBuilding ^ >
    WrappedImageBuilding;
  msclr::auto_gcroot< IntersonArray::Imaging::Capture ^ >
    WrappedCapture;

  gcroot< ArrayType ^ >                         Buffer;
  gcroot< RFArrayType ^ >                       RFBuffer;
  gcroot< NewImageHandler ^ >                   Handler;
  gcroot< NewRFImageHandler ^ >                 RFHandler;

  gcroot< IntersonArray::Imaging::Capture::NewImageHandler ^ >
    HandlerDelegate;
  gcroot< IntersonArray::Imaging::Capture::NewImageHandler ^ >
    RFHandlerDelegate;

};

#pragma unmanaged

Container
::Container():
  Impl( new ContainerImpl() )
{
}

Container
::~Container()
{
  delete Impl;
}

bool
Container
::GetCompound()
{
  return Impl->GetCompound();
}

void
Container
::SetCompound( bool value )
{
  Impl->SetCompound( value );
}

bool
Container
::GetDoubler()
{
  return Impl->GetDoubler();
}

void
Container
::SetDoubler( bool value )
{
  Impl->SetDoubler( value );
}

int
Container
::GetHeightScan() const
{
  return Impl->GetHeightScan();
}

float
Container
::GetMmPerPixel() const
{
  return Impl->GetMmPerPixel();
}

double
Container
::GetTrueDepth() const
{
  return Impl->GetMmPerPixel();
}

int
Container
::GetWidthScan() const
{
  return Impl->GetWidthScan();
}

int
Container
::GetZeroOfYScale() const
{
  return Impl->GetZeroOfYScale();
}

Container::ScanConverterError
Container
::HardInitScanConverter( int depth, int widthScan, int heightScan,
  int steering )
{
  return Impl->HardInitScanConverter( depth, widthScan, heightScan,
    steering );
}

Container::ScanConverterError
Container
::IdleInitScanConverter( int depth, int width, int height, short idleId,
  int idleSteering, bool idleDoubler, bool idleCompound,
  int idleCompoundAngle )
{
  return Impl->IdleInitScanConverter( depth, width, height, idleId,
    idleSteering, idleDoubler, idleCompound, idleCompoundAngle );
}

//
// Begin Capture
//
bool
Container
::GetRFData()
{
  return Impl->GetRFData();
}

void
Container
::SetRFData( bool transferOn )
{
  Impl->SetRFData( transferOn );
}

bool
Container
::GetFrameAvg()
{
  return Impl->GetFrameAvg();
}

void
Container
::SetFrameAvg( bool doAveraging )
{
  Impl->SetFrameAvg( doAveraging );
}

bool
Container
::GetScanOn() const
{
  return Impl->GetScanOn();
}

void
Container
::AbortScan()
{
  Impl->AbortScan();
}

void
Container
::DisposeScan()
{
  Impl->DisposeScan();
}

void
Container
::StartReadScan()
{
  Impl->StartReadScan();
}

void
Container
::StartRFReadScan()
{
  Impl->StartRFReadScan();
}

void
Container
::StopReadScan()
{
  Impl->StopReadScan();
}

void
Container
::SetNewImageCallback( NewImageCallbackType callback,
  void *clientData )
{
  Impl->SetNewImageCallback( callback, clientData );
}

void
Container
::SetNewRFImageCallback( NewRFImageCallbackType callback,
  void *clientData )
{
  Impl->SetNewRFImageCallback( callback, clientData );
}

void
Container
::SetHWControls(IntersonArrayCxx::Controls::HWControls * controls)
{
  Impl->SetHWControls(controls);
}

} // end namespace Imaging

} // end namespace IntersonArrayCxx
<|endoftext|>


<commit_before>/* Copyright 2013 - 2015 Nikita Batov, Kseniya Gonta and CyberTech Labs Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "analogSensor.h"

#include <trikKernel/configurer.h>
#include <QsLog.h>

#include "i2cCommunicator.h"
#include "configurerHelper.h"

using namespace trikControl;

AnalogSensor::AnalogSensor(const QString &port, const trikKernel::Configurer &configurer, I2cCommunicator &communicator)
	: mCommunicator(communicator)
{
	mI2cCommandNumber = ConfigurerHelper::configureInt(configurer, mState, port, "i2cCommandNumber");
	mIRType = configurer.attributeByPort(port, "type") == "SharpGP2" ? Type::sharpGP2 : Type::analog;
	// We use linear subjection to normalize common analog sensor values:
	// normalizedValue = k * rawValue + b
	// To calculate k and b we need two raw values and two corresponding them normalized values.

	// We use hyperbolical subjection to normalize SharpGP2 sensor values:
	// normalizedValue = s / (rawValue + l) + n
	// To calculate s, l and n we need sensor readings at three distances.

	if (mIRType == Type::sharpGP2){
		calculateLNS(port, configurer);
	} else {
		calculateKB(port, configurer);
	}
	mState.ready();
}

AnalogSensor::Status AnalogSensor::status() const
{
	return DeviceInterface::combine(mCommunicator, mState.status());
}

int AnalogSensor::read()
{
	const auto raw = readRawData();
	if (mIRType == Type::sharpGP2) {
		const auto quotient = raw + mL;
                const auto result = quotient != 0 ? mS / quotient + mN : 0;
		return result;
	}
	return mK * raw + mB;
}

int AnalogSensor::readRawData()
{
	if (!mState.isReady() || mCommunicator.status() != DeviceInterface::Status::ready) {
		return 0;
	}

	QByteArray command(1, '\0');
	command[0] = static_cast<char>(mI2cCommandNumber & 0xFF);

	return mCommunicator.read(command);
}

void AnalogSensor::calculateLNS(const QString &port, const trikKernel::Configurer &configurer)
{
	QStringList result;
	for (const QString &str : configurer.attributeByPort(port, "values").split(")")) {
		result += str.mid(1).split(";");
	}

	const int x1 = result[0].toInt();
	const int y1 = result[1].toInt();
	const int x2 = result[2].toInt();
	const int y2 = result[3].toInt();
	const int x3 = result[4].toInt();
	const int y3 = result[5].toInt();

	// Counted from equations x1 = mS/(y1 + mL) + mN, x2 = mS/(y2 + mL) + mN, x3 = mS/(y3 + mL) + mN
	mL = (-x1 * y1 * y3 - x3 * y2 * y3 + x3 * y1 * y3 + x1 * y1 * y2 + x2 * y2 * y3 - x2 * y1 * y2) /
			(x1 * y3 - x2 * y3 + x2 * y1 - x1 * y2 + x3 * y2 - x3 * y1);
	mS = (x1 - x2) * (y1 + mL) * (y2 + mL) / (y2 - y1);
	mN = x1 - mS / (y1 + mL);
}

void AnalogSensor::calculateKB(const QString &port, const trikKernel::Configurer &configurer)
{
	const int rawValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue1");
	const int rawValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue2");
	const int normalizedValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue1");
	const int normalizedValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue2");
	if (rawValue1 == rawValue2) {
		QLOG_ERROR() <<  "Sensor calibration error: rawValue1 = rawValue2!";
		mState.fail();
		mK = 0;
		mB = 0;
	} else {
		mK = static_cast<qreal>(normalizedValue2 - normalizedValue1) / (rawValue2 - rawValue1);
		mB = normalizedValue1 - mK * rawValue1;
	}
}
<commit_msg>Some corrections<commit_after>/* Copyright 2013 - 2015 Nikita Batov, Kseniya Gonta and CyberTech Labs Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "analogSensor.h"

#include <trikKernel/configurer.h>
#include <QsLog.h>

#include "i2cCommunicator.h"
#include "configurerHelper.h"

using namespace trikControl;

AnalogSensor::AnalogSensor(const QString &port, const trikKernel::Configurer &configurer, I2cCommunicator &communicator)
	: mCommunicator(communicator)
{
	mI2cCommandNumber = ConfigurerHelper::configureInt(configurer, mState, port, "i2cCommandNumber");
	mIRType = configurer.attributeByPort(port, "type") == "SharpGP2" ? Type::sharpGP2 : Type::analog;
	// We use linear subjection to normalize common analog sensor values:
	// normalizedValue = k * rawValue + b
	// To calculate k and b we need two raw values and two corresponding them normalized values.

	// We use hyperbolical subjection to normalize SharpGP2 sensor values:
	// normalizedValue = s / (rawValue + l) + n
	// To calculate s, l and n we need sensor readings at three distances.

	if (mIRType == Type::sharpGP2){
		calculateLNS(port, configurer);
	} else {
		calculateKB(port, configurer);
	}
	mState.ready();
}

AnalogSensor::Status AnalogSensor::status() const
{
	return DeviceInterface::combine(mCommunicator, mState.status());
}

int AnalogSensor::read()
{
	const auto raw = readRawData();
	if (mIRType == Type::sharpGP2) {
		const auto quotient = raw + mL;
		const auto result = quotient != 0 ? mS / quotient + mN : 0;
		return result;
	}

	return mK * raw + mB;
}

int AnalogSensor::readRawData()
{
	if (!mState.isReady() || mCommunicator.status() != DeviceInterface::Status::ready) {
		return 0;
	}

	QByteArray command(1, '\0');
	command[0] = static_cast<char>(mI2cCommandNumber & 0xFF);

	return mCommunicator.read(command);
}

void AnalogSensor::calculateLNS(const QString &port, const trikKernel::Configurer &configurer)
{
	QStringList result;
	for (const QString &str : configurer.attributeByPort(port, "values").split(")")) {
		result += str.mid(1).split(";");
	}

	const int x1 = result[0].toInt();
	const int y1 = result[1].toInt();
	const int x2 = result[2].toInt();
	const int y2 = result[3].toInt();
	const int x3 = result[4].toInt();
	const int y3 = result[5].toInt();

	// Counted from equations x1 = mS/(y1 + mL) + mN, x2 = mS/(y2 + mL) + mN, x3 = mS/(y3 + mL) + mN
	mL = (-x1 * y1 * y3 - x3 * y2 * y3 + x3 * y1 * y3 + x1 * y1 * y2 + x2 * y2 * y3 - x2 * y1 * y2) /
			(x1 * y3 - x2 * y3 + x2 * y1 - x1 * y2 + x3 * y2 - x3 * y1);
	mS = (x1 - x2) * (y1 + mL) * (y2 + mL) / (y2 - y1);
	mN = x1 - mS / (y1 + mL);
}

void AnalogSensor::calculateKB(const QString &port, const trikKernel::Configurer &configurer)
{
	const int rawValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue1");
	const int rawValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue2");
	const int normalizedValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue1");
	const int normalizedValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue2");
	if (rawValue1 == rawValue2) {
		QLOG_ERROR() <<  "Sensor calibration error: rawValue1 = rawValue2!";
		mState.fail();
		mK = 0;
		mB = 0;
	} else {
		mK = static_cast<qreal>(normalizedValue2 - normalizedValue1) / (rawValue2 - rawValue1);
		mB = normalizedValue1 - mK * rawValue1;
	}
}
<|endoftext|>


<commit_before>// Author: Jakob Blomer CERN  07/2018

/*************************************************************************
 * Copyright (C) 1995-2017, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

// clang-format off
/** \class ROOT::RDF::RSqliteDS
    \ingroup dataframe
    \brief RDataFrame data source class for reading SQlite files.
*/
// clang-format on

#include <ROOT/RSqliteDS.hxx>
#include <ROOT/RDFUtils.hxx>
#include <ROOT/RMakeUnique.hxx>

#include <TError.h>

#include <algorithm>
#include <cstring>

namespace ROOT {

namespace RDF {

RSqliteDS::RSqliteDS(std::string_view fileName, std::string_view query)
   : fDb(NULL)
   , fQuery(NULL)
   , fNRow(0)
{
   int retval;

   retval = sqlite3_open_v2(std::string(fileName).c_str(), &fDb,
     SQLITE_OPEN_READONLY | SQLITE_OPEN_FULLMUTEX, NULL);
   if (retval != SQLITE_OK) SqliteError(retval);

   retval = sqlite3_prepare_v2(fDb, std::string(query).c_str(), -1, &fQuery, NULL);
   if (retval != SQLITE_OK) SqliteError(retval);

   int colCount = sqlite3_column_count(fQuery);
   retval = sqlite3_step(fQuery);
   if ((retval != SQLITE_ROW) && (retval != SQLITE_DONE)) SqliteError(retval);

   fValues.resize(colCount);
   for (int i = 0; i < colCount; ++i) {
      fColumnNames.emplace_back(sqlite3_column_name(fQuery, i));
      int type = SQLITE_NULL;
      // Try first with the declared column type and then with the dynamic type
      // for expressions
      std::string declType = sqlite3_column_decltype(fQuery, i);
      if (declType == "INTEGER") type = SQLITE_INTEGER;
      else if (declType == "FLOAT") type = SQLITE_FLOAT;
      else if (declType == "TEXT") type = SQLITE_TEXT;
      else if (declType == "BLOB") type = SQLITE_BLOB;
      else if (declType == "NULL" && retval == SQLITE_ROW) type = sqlite3_column_type(fQuery, i);

      switch (type) {
         case SQLITE_INTEGER:
            fColumnTypes.push_back(Types::kInteger);
            fValues[i].fType = Types::kInteger;
            fValues[i].fPtr = &fValues[i].fInteger;
            break;
         case SQLITE_FLOAT:
            fColumnTypes.push_back(Types::kReal);
            fValues[i].fType = Types::kReal;
            fValues[i].fPtr = &fValues[i].fReal;
            break;
         case SQLITE_TEXT:
            fColumnTypes.push_back(Types::kText);
            fValues[i].fType = Types::kText;
            fValues[i].fPtr = &fValues[i].fText;
            break;
         case SQLITE_BLOB:
            fColumnTypes.push_back(Types::kBlob);
            fValues[i].fType = Types::kBlob;
            fValues[i].fPtr = &fValues[i].fBlob;
            break;
         case SQLITE_NULL:
            // TODO: Null values in first rows are not well handled
            fColumnTypes.push_back(Types::kNull);
            fValues[i].fType = Types::kNull;
            fValues[i].fPtr = &fValues[i].fNull;
            break;
         default:
            throw std::runtime_error("Unhandled data type");
      }
   }

   fTypeNames[Types::kInteger] = "Long64_t";
   fTypeNames[Types::kReal] = "double";
   fTypeNames[Types::kText] = "std::string";
   fTypeNames[Types::kBlob] = "std::vector<unsigned char>";
   fTypeNames[Types::kNull] = "void *";
}

RSqliteDS::~RSqliteDS()
{
   sqlite3_finalize(fQuery);
   sqlite3_close_v2(fDb);
}

const std::vector<std::string> &RSqliteDS::GetColumnNames() const
{
   return fColumnNames;
}

RDataSource::Record_t RSqliteDS::GetColumnReadersImpl(std::string_view name, const std::type_info &ti)
{
   const auto index = std::distance(fColumnNames.begin(), std::find(fColumnNames.begin(), fColumnNames.end(), name));
   Types type = fColumnTypes[index];

   if ((type == Types::kReal && typeid(double) != ti) ||
       (type == Types::kInteger && typeid(Long64_t) != ti) ||
       (type == Types::kText && typeid(std::string) != ti) ||
       (type == Types::kBlob && typeid(std::vector<unsigned char>) != ti) ||
       (type == Types::kNull && typeid(void *) != ti))
   {
      std::string errmsg = "The type selected for column \"";
      errmsg += name;
      errmsg += "\" does not correspond to column type, which is ";
      errmsg += GetTypeName(name);
      throw std::runtime_error(errmsg);
   }

   fValues[index].fIsActive = true;
   std::vector<void *> result;
   result.push_back(&fValues[index].fPtr);
   return result;
}

std::vector<std::pair<ULong64_t, ULong64_t>> RSqliteDS::GetEntryRanges()
{
   std::vector<std::pair<ULong64_t, ULong64_t>> entryRanges;
   int retval = sqlite3_step(fQuery);
   switch (retval) {
   case SQLITE_DONE:
      return entryRanges;
   case SQLITE_ROW:
      entryRanges.emplace_back(fNRow, fNRow + 1);
      fNRow++;
      return entryRanges;
   default:
      SqliteError(retval);
      // Never here
      abort();
   }
}

std::string RSqliteDS::GetTypeName(std::string_view colName) const
{
   unsigned N = fColumnNames.size();

   for (unsigned i = 0; i < N; ++i) {
      if (colName == fColumnNames[i]) {
         return fTypeNames.at(fColumnTypes[i]);
      }
   }
   throw std::runtime_error("Unknown column: " + std::string(colName));
}

bool RSqliteDS::HasColumn(std::string_view colName) const
{
   return std::find(fColumnNames.begin(), fColumnNames.end(), colName) !=
          fColumnNames.end();
}

void RSqliteDS::Initialise() {
   fNRow = 0;
   int retval = sqlite3_reset(fQuery);
   if (retval != SQLITE_OK) throw std::runtime_error("SQlite error, reset");
}

RDataFrame MakeSqliteDataFrame(std::string_view fileName, std::string_view query)
{
   ROOT::RDataFrame tdf(std::make_unique<RSqliteDS>(fileName, query));
   return tdf;
}

bool RSqliteDS::SetEntry(unsigned int /* slot */, ULong64_t entry)
{
   R__ASSERT(entry + 1 == fNRow);
   unsigned N = fValues.size();
   for (unsigned i = 0; i < N; ++i) {
     if (!fValues[i].fIsActive) continue;

     int nbytes;
     switch (fValues[i].fType) {
     case Types::kInteger:
        fValues[i].fInteger = sqlite3_column_int64(fQuery, i);
        break;
     case Types::kReal:
        fValues[i].fReal = sqlite3_column_double(fQuery, i);
        break;
     case Types::kText:
        nbytes = sqlite3_column_bytes(fQuery, i);
        if (nbytes == 0) {
           fValues[i].fText = "";
        } else {
           fValues[i].fText = reinterpret_cast<const char *>(
             sqlite3_column_text(fQuery, i));
        }
        break;
     case Types::kBlob:
        nbytes = sqlite3_column_bytes(fQuery, i);
        fValues[i].fBlob.resize(nbytes);
        if (nbytes > 0) {
           std::memcpy(fValues[i].fBlob.data(), sqlite3_column_blob(fQuery, i), nbytes);
        }
        break;
     case Types::kNull:
        break;
     default:
       throw std::runtime_error("Unhandled column type");
     }
   }
   return true;
}

void RSqliteDS::SetNSlots(unsigned int /* nSlots */)
{
}

void RSqliteDS::SqliteError(int errcode) {
   std::string errmsg = "SQlite error: ";
   errmsg += sqlite3_errstr(errcode);
   throw std::runtime_error(errmsg);
}

} // ns RDF

} // ns ROOT
<commit_msg>[DF] Fix expression columns in SQlite data source<commit_after>// Author: Jakob Blomer CERN  07/2018

/*************************************************************************
 * Copyright (C) 1995-2017, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

// clang-format off
/** \class ROOT::RDF::RSqliteDS
    \ingroup dataframe
    \brief RDataFrame data source class for reading SQlite files.
*/
// clang-format on

#include <ROOT/RSqliteDS.hxx>
#include <ROOT/RDFUtils.hxx>
#include <ROOT/RMakeUnique.hxx>

#include <TError.h>

#include <algorithm>
#include <cstring>

namespace ROOT {

namespace RDF {

RSqliteDS::RSqliteDS(std::string_view fileName, std::string_view query)
   : fDb(NULL)
   , fQuery(NULL)
   , fNRow(0)
{
   int retval;

   retval = sqlite3_open_v2(std::string(fileName).c_str(), &fDb,
     SQLITE_OPEN_READONLY | SQLITE_OPEN_FULLMUTEX, NULL);
   if (retval != SQLITE_OK) SqliteError(retval);

   retval = sqlite3_prepare_v2(fDb, std::string(query).c_str(), -1, &fQuery, NULL);
   if (retval != SQLITE_OK) SqliteError(retval);

   int colCount = sqlite3_column_count(fQuery);
   retval = sqlite3_step(fQuery);
   if ((retval != SQLITE_ROW) && (retval != SQLITE_DONE)) SqliteError(retval);

   fValues.resize(colCount);
   for (int i = 0; i < colCount; ++i) {
      fColumnNames.emplace_back(sqlite3_column_name(fQuery, i));
      int type = SQLITE_NULL;
      // Try first with the declared column type and then with the dynamic type
      // for expressions
      const char *declTypeCstr = sqlite3_column_decltype(fQuery, i);
      if (declTypeCstr == NULL) {
         if (retval == SQLITE_ROW) type = sqlite3_column_type(fQuery, i);
      } else {
         std::string declType(declTypeCstr);
         if (declType == "INTEGER") type = SQLITE_INTEGER;
         else if (declType == "FLOAT") type = SQLITE_FLOAT;
         else if (declType == "TEXT") type = SQLITE_TEXT;
         else if (declType == "BLOB") type = SQLITE_BLOB;
         else throw std::runtime_error("Unexpected column decl type");
      }

      switch (type) {
         case SQLITE_INTEGER:
            fColumnTypes.push_back(Types::kInteger);
            fValues[i].fType = Types::kInteger;
            fValues[i].fPtr = &fValues[i].fInteger;
            break;
         case SQLITE_FLOAT:
            fColumnTypes.push_back(Types::kReal);
            fValues[i].fType = Types::kReal;
            fValues[i].fPtr = &fValues[i].fReal;
            break;
         case SQLITE_TEXT:
            fColumnTypes.push_back(Types::kText);
            fValues[i].fType = Types::kText;
            fValues[i].fPtr = &fValues[i].fText;
            break;
         case SQLITE_BLOB:
            fColumnTypes.push_back(Types::kBlob);
            fValues[i].fType = Types::kBlob;
            fValues[i].fPtr = &fValues[i].fBlob;
            break;
         case SQLITE_NULL:
            // TODO: Null values in first rows are not well handled
            fColumnTypes.push_back(Types::kNull);
            fValues[i].fType = Types::kNull;
            fValues[i].fPtr = &fValues[i].fNull;
            break;
         default:
            throw std::runtime_error("Unhandled data type");
      }
   }

   fTypeNames[Types::kInteger] = "Long64_t";
   fTypeNames[Types::kReal] = "double";
   fTypeNames[Types::kText] = "std::string";
   fTypeNames[Types::kBlob] = "std::vector<unsigned char>";
   fTypeNames[Types::kNull] = "void *";
}

RSqliteDS::~RSqliteDS()
{
   sqlite3_finalize(fQuery);
   sqlite3_close_v2(fDb);
}

const std::vector<std::string> &RSqliteDS::GetColumnNames() const
{
   return fColumnNames;
}

RDataSource::Record_t RSqliteDS::GetColumnReadersImpl(std::string_view name, const std::type_info &ti)
{
   const auto index = std::distance(fColumnNames.begin(), std::find(fColumnNames.begin(), fColumnNames.end(), name));
   Types type = fColumnTypes[index];

   if ((type == Types::kReal && typeid(double) != ti) ||
       (type == Types::kInteger && typeid(Long64_t) != ti) ||
       (type == Types::kText && typeid(std::string) != ti) ||
       (type == Types::kBlob && typeid(std::vector<unsigned char>) != ti) ||
       (type == Types::kNull && typeid(void *) != ti))
   {
      std::string errmsg = "The type selected for column \"";
      errmsg += name;
      errmsg += "\" does not correspond to column type, which is ";
      errmsg += GetTypeName(name);
      throw std::runtime_error(errmsg);
   }

   fValues[index].fIsActive = true;
   std::vector<void *> result;
   result.push_back(&fValues[index].fPtr);
   return result;
}

std::vector<std::pair<ULong64_t, ULong64_t>> RSqliteDS::GetEntryRanges()
{
   std::vector<std::pair<ULong64_t, ULong64_t>> entryRanges;
   int retval = sqlite3_step(fQuery);
   switch (retval) {
   case SQLITE_DONE:
      return entryRanges;
   case SQLITE_ROW:
      entryRanges.emplace_back(fNRow, fNRow + 1);
      fNRow++;
      return entryRanges;
   default:
      SqliteError(retval);
      // Never here
      abort();
   }
}

std::string RSqliteDS::GetTypeName(std::string_view colName) const
{
   unsigned N = fColumnNames.size();

   for (unsigned i = 0; i < N; ++i) {
      if (colName == fColumnNames[i]) {
         return fTypeNames.at(fColumnTypes[i]);
      }
   }
   throw std::runtime_error("Unknown column: " + std::string(colName));
}

bool RSqliteDS::HasColumn(std::string_view colName) const
{
   return std::find(fColumnNames.begin(), fColumnNames.end(), colName) !=
          fColumnNames.end();
}

void RSqliteDS::Initialise() {
   fNRow = 0;
   int retval = sqlite3_reset(fQuery);
   if (retval != SQLITE_OK) throw std::runtime_error("SQlite error, reset");
}

RDataFrame MakeSqliteDataFrame(std::string_view fileName, std::string_view query)
{
   ROOT::RDataFrame tdf(std::make_unique<RSqliteDS>(fileName, query));
   return tdf;
}

bool RSqliteDS::SetEntry(unsigned int /* slot */, ULong64_t entry)
{
   R__ASSERT(entry + 1 == fNRow);
   unsigned N = fValues.size();
   for (unsigned i = 0; i < N; ++i) {
     if (!fValues[i].fIsActive) continue;

     int nbytes;
     switch (fValues[i].fType) {
     case Types::kInteger:
        fValues[i].fInteger = sqlite3_column_int64(fQuery, i);
        break;
     case Types::kReal:
        fValues[i].fReal = sqlite3_column_double(fQuery, i);
        break;
     case Types::kText:
        nbytes = sqlite3_column_bytes(fQuery, i);
        if (nbytes == 0) {
           fValues[i].fText = "";
        } else {
           fValues[i].fText = reinterpret_cast<const char *>(
             sqlite3_column_text(fQuery, i));
        }
        break;
     case Types::kBlob:
        nbytes = sqlite3_column_bytes(fQuery, i);
        fValues[i].fBlob.resize(nbytes);
        if (nbytes > 0) {
           std::memcpy(fValues[i].fBlob.data(), sqlite3_column_blob(fQuery, i), nbytes);
        }
        break;
     case Types::kNull:
        break;
     default:
       throw std::runtime_error("Unhandled column type");
     }
   }
   return true;
}

void RSqliteDS::SetNSlots(unsigned int /* nSlots */)
{
}

void RSqliteDS::SqliteError(int errcode) {
   std::string errmsg = "SQlite error: ";
   errmsg += sqlite3_errstr(errcode);
   throw std::runtime_error(errmsg);
}

} // ns RDF

} // ns ROOT
<|endoftext|>


<commit_before>
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "MissionCommandTree.h"
#include "FactMetaData.h"
#include "Vehicle.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "QGroundControlQmlGlobal.h"
#include "MissionCommandUIInfo.h"
#include "MissionCommandList.h"

#include <QQmlEngine>

MissionCommandTree::MissionCommandTree(QGCApplication* app, bool unitTest)
    : QGCTool(app)
    , _unitTest(unitTest)
{
}

void MissionCommandTree::setToolbox(QGCToolbox* toolbox)
{
    QGCTool::setToolbox(toolbox);

#ifdef UNITTEST_BUILD
    if (_unitTest) {
        // Load unit testing tree
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC] =           new MissionCommandList(":/json/unittest/MavCmdInfoCommon.json", true, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_FIXED_WING] =        new MissionCommandList(":/json/unittest/MavCmdInfoFixedWing.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_QUADROTOR] =         new MissionCommandList(":/json/unittest/MavCmdInfoMultiRotor.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_VTOL_QUADROTOR] =    new MissionCommandList(":/json/unittest/MavCmdInfoVTOL.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_SUBMARINE] =         new MissionCommandList(":/json/unittest/MavCmdInfoSub.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GROUND_ROVER] =      new MissionCommandList(":/json/unittest/MavCmdInfoRover.json", false, this);
    } else {
#endif
        // Load all levels of hierarchy
        foreach (MAV_AUTOPILOT firmwareType, _toolbox->firmwarePluginManager()->knownFirmwareTypes()) {
            FirmwarePlugin* plugin = _toolbox->firmwarePluginManager()->firmwarePluginForAutopilot(firmwareType, MAV_TYPE_QUADROTOR);

            QList<MAV_TYPE> vehicleTypes;
            vehicleTypes << MAV_TYPE_GENERIC << MAV_TYPE_FIXED_WING << MAV_TYPE_QUADROTOR << MAV_TYPE_VTOL_QUADROTOR << MAV_TYPE_GROUND_ROVER << MAV_TYPE_SUBMARINE;

            foreach(MAV_TYPE vehicleType, vehicleTypes) {
                QString overrideFile = plugin->missionCommandOverrides(vehicleType);
                if (!overrideFile.isEmpty()) {
                    _staticCommandTree[firmwareType][vehicleType] = new MissionCommandList(overrideFile, firmwareType == MAV_AUTOPILOT_GENERIC && vehicleType == MAV_TYPE_GENERIC /* baseCommandList */, this);
                }
            }
        }
#ifdef UNITTEST_BUILD
    }
#endif
}

MAV_AUTOPILOT MissionCommandTree::_baseFirmwareType(MAV_AUTOPILOT firmwareType) const
{
    if (qgcApp()->toolbox()->firmwarePluginManager()->knownFirmwareTypes().contains(firmwareType)) {
        return firmwareType;
    } else {
        return MAV_AUTOPILOT_GENERIC;
    }

}

MAV_TYPE MissionCommandTree::_baseVehicleType(MAV_TYPE mavType) const
{
    if (QGCMAVLink::isFixedWing(mavType)) {
        return MAV_TYPE_FIXED_WING;
    } else if (QGCMAVLink::isMultiRotor(mavType)) {
        return MAV_TYPE_QUADROTOR;
    } else if (QGCMAVLink::isVTOL(mavType)) {
        return MAV_TYPE_VTOL_QUADROTOR;
    } else if (QGCMAVLink::isRover(mavType)) {
        return MAV_TYPE_GROUND_ROVER;
    } else if (QGCMAVLink::isSub(mavType)) {
        return MAV_TYPE_SUBMARINE;
    } else {
        return MAV_TYPE_GENERIC;
    }
}

/// Add the next level of the hierarchy to a collapsed tree.
///     @param vehicle Collapsed tree is for this vehicle
///     @param cmdList List of mission commands to collapse into ui info
///     @param collapsedTree Tree we are collapsing into
void MissionCommandTree::_collapseHierarchy(Vehicle*                                vehicle,
                                            const MissionCommandList*               cmdList,
                                            QMap<MAV_CMD, MissionCommandUIInfo*>&   collapsedTree)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);

    foreach (MAV_CMD command, cmdList->commandIds()) {
        MissionCommandUIInfo* uiInfo = cmdList->getUIInfo(command);
        if (uiInfo) {
            if (collapsedTree.contains(command)) {
                collapsedTree[command]->_overrideInfo(uiInfo);
            } else {
                collapsedTree[command] = new MissionCommandUIInfo(*uiInfo);
            }
        }
    }
}

void MissionCommandTree::_buildAvailableCommands(Vehicle* vehicle)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);

    if (_availableCommands.contains(baseFirmwareType) &&
            _availableCommands[baseFirmwareType].contains(baseVehicleType)) {
        // Available commands list already built
        return;
    }

    // Build new available commands list

    QMap<MAV_CMD, MissionCommandUIInfo*>& collapsedTree = _availableCommands[baseFirmwareType][baseVehicleType];

    // Any Firmware, Any Vehicle
    _collapseHierarchy(vehicle, _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC], collapsedTree);

    // Any Firmware, Specific Vehicle
    if (baseVehicleType != MAV_TYPE_GENERIC) {
        _collapseHierarchy(vehicle, _staticCommandTree[MAV_AUTOPILOT_GENERIC][baseVehicleType], collapsedTree);
    }

    // Known Firmware, Any Vehicle
    if (baseFirmwareType != MAV_AUTOPILOT_GENERIC) {
        _collapseHierarchy(vehicle, _staticCommandTree[baseFirmwareType][MAV_TYPE_GENERIC], collapsedTree);

        // Known Firmware, Specific Vehicle
        if (baseVehicleType != MAV_TYPE_GENERIC) {
            _collapseHierarchy(vehicle, _staticCommandTree[baseFirmwareType][baseVehicleType], collapsedTree);
        }
    }

    // Build category list
    QMapIterator<MAV_CMD, MissionCommandUIInfo*> iter(collapsedTree);
    while (iter.hasNext()) {
        iter.next();
        QString newCategory = iter.value()->category();
        if (!_availableCategories[baseFirmwareType][baseVehicleType].contains(newCategory)) {
            _availableCategories[baseFirmwareType][baseVehicleType].append(newCategory);
        }
    }
}

QStringList MissionCommandTree::_availableCategoriesForVehicle(Vehicle* vehicle)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
    _buildAvailableCommands(vehicle);

    return _availableCategories[baseFirmwareType][baseVehicleType];
}

QString MissionCommandTree::friendlyName(MAV_CMD command)
{
    MissionCommandList *    commandList =   _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC];
    MissionCommandUIInfo*   uiInfo =        commandList->getUIInfo(command);

    return uiInfo->friendlyName();
}

QString MissionCommandTree::rawName(MAV_CMD command)
{
    MissionCommandList *    commandList =   _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC];
    MissionCommandUIInfo*   uiInfo =        commandList->getUIInfo(command);

    return uiInfo->rawName();
}

const QList<MAV_CMD>& MissionCommandTree::allCommandIds(void) const
{
    return _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC]->commandIds();
}

const MissionCommandUIInfo* MissionCommandTree::getUIInfo(Vehicle* vehicle, MAV_CMD command)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
    _buildAvailableCommands(vehicle);

    const QMap<MAV_CMD, MissionCommandUIInfo*>& infoMap = _availableCommands[baseFirmwareType][baseVehicleType];
    if (infoMap.contains(command)) {
        return infoMap[command];
    } else {
        return NULL;
    }
}

QVariantList MissionCommandTree::getCommandsForCategory(Vehicle* vehicle, const QString& category)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
    _buildAvailableCommands(vehicle);

    QVariantList list;
    QMap<MAV_CMD, MissionCommandUIInfo*> commandMap = _availableCommands[baseFirmwareType][baseVehicleType];
    foreach (MAV_CMD command, commandMap.keys()) {
        MissionCommandUIInfo* uiInfo = commandMap[command];
        if (uiInfo->category() == category) {
            list.append(QVariant::fromValue(uiInfo));
        }
    }

    return list;
}

void MissionCommandTree::_baseVehicleInfo(Vehicle* vehicle, MAV_AUTOPILOT& baseFirmwareType, MAV_TYPE& baseVehicleType) const
{
    if (vehicle) {
        baseFirmwareType = _baseFirmwareType(vehicle->firmwareType());
        baseVehicleType = _baseVehicleType(vehicle->vehicleType());
    } else {
        // No Vehicle means offline editing
        baseFirmwareType = _baseFirmwareType((MAV_AUTOPILOT)QGroundControlQmlGlobal::offlineEditingFirmwareType()->rawValue().toInt());
        baseVehicleType = _baseVehicleType((MAV_TYPE)QGroundControlQmlGlobal::offlineEditingVehicleType()->rawValue().toInt());
    }
}
<commit_msg>Fix crash on unknown command<commit_after>
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "MissionCommandTree.h"
#include "FactMetaData.h"
#include "Vehicle.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "QGroundControlQmlGlobal.h"
#include "MissionCommandUIInfo.h"
#include "MissionCommandList.h"

#include <QQmlEngine>

MissionCommandTree::MissionCommandTree(QGCApplication* app, bool unitTest)
    : QGCTool(app)
    , _unitTest(unitTest)
{
}

void MissionCommandTree::setToolbox(QGCToolbox* toolbox)
{
    QGCTool::setToolbox(toolbox);

#ifdef UNITTEST_BUILD
    if (_unitTest) {
        // Load unit testing tree
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC] =           new MissionCommandList(":/json/unittest/MavCmdInfoCommon.json", true, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_FIXED_WING] =        new MissionCommandList(":/json/unittest/MavCmdInfoFixedWing.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_QUADROTOR] =         new MissionCommandList(":/json/unittest/MavCmdInfoMultiRotor.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_VTOL_QUADROTOR] =    new MissionCommandList(":/json/unittest/MavCmdInfoVTOL.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_SUBMARINE] =         new MissionCommandList(":/json/unittest/MavCmdInfoSub.json", false, this);
        _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GROUND_ROVER] =      new MissionCommandList(":/json/unittest/MavCmdInfoRover.json", false, this);
    } else {
#endif
        // Load all levels of hierarchy
        foreach (MAV_AUTOPILOT firmwareType, _toolbox->firmwarePluginManager()->knownFirmwareTypes()) {
            FirmwarePlugin* plugin = _toolbox->firmwarePluginManager()->firmwarePluginForAutopilot(firmwareType, MAV_TYPE_QUADROTOR);

            QList<MAV_TYPE> vehicleTypes;
            vehicleTypes << MAV_TYPE_GENERIC << MAV_TYPE_FIXED_WING << MAV_TYPE_QUADROTOR << MAV_TYPE_VTOL_QUADROTOR << MAV_TYPE_GROUND_ROVER << MAV_TYPE_SUBMARINE;

            foreach(MAV_TYPE vehicleType, vehicleTypes) {
                QString overrideFile = plugin->missionCommandOverrides(vehicleType);
                if (!overrideFile.isEmpty()) {
                    _staticCommandTree[firmwareType][vehicleType] = new MissionCommandList(overrideFile, firmwareType == MAV_AUTOPILOT_GENERIC && vehicleType == MAV_TYPE_GENERIC /* baseCommandList */, this);
                }
            }
        }
#ifdef UNITTEST_BUILD
    }
#endif
}

MAV_AUTOPILOT MissionCommandTree::_baseFirmwareType(MAV_AUTOPILOT firmwareType) const
{
    if (qgcApp()->toolbox()->firmwarePluginManager()->knownFirmwareTypes().contains(firmwareType)) {
        return firmwareType;
    } else {
        return MAV_AUTOPILOT_GENERIC;
    }

}

MAV_TYPE MissionCommandTree::_baseVehicleType(MAV_TYPE mavType) const
{
    if (QGCMAVLink::isFixedWing(mavType)) {
        return MAV_TYPE_FIXED_WING;
    } else if (QGCMAVLink::isMultiRotor(mavType)) {
        return MAV_TYPE_QUADROTOR;
    } else if (QGCMAVLink::isVTOL(mavType)) {
        return MAV_TYPE_VTOL_QUADROTOR;
    } else if (QGCMAVLink::isRover(mavType)) {
        return MAV_TYPE_GROUND_ROVER;
    } else if (QGCMAVLink::isSub(mavType)) {
        return MAV_TYPE_SUBMARINE;
    } else {
        return MAV_TYPE_GENERIC;
    }
}

/// Add the next level of the hierarchy to a collapsed tree.
///     @param vehicle Collapsed tree is for this vehicle
///     @param cmdList List of mission commands to collapse into ui info
///     @param collapsedTree Tree we are collapsing into
void MissionCommandTree::_collapseHierarchy(Vehicle*                                vehicle,
                                            const MissionCommandList*               cmdList,
                                            QMap<MAV_CMD, MissionCommandUIInfo*>&   collapsedTree)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);

    foreach (MAV_CMD command, cmdList->commandIds()) {
        MissionCommandUIInfo* uiInfo = cmdList->getUIInfo(command);
        if (uiInfo) {
            if (collapsedTree.contains(command)) {
                collapsedTree[command]->_overrideInfo(uiInfo);
            } else {
                collapsedTree[command] = new MissionCommandUIInfo(*uiInfo);
            }
        }
    }
}

void MissionCommandTree::_buildAvailableCommands(Vehicle* vehicle)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);

    if (_availableCommands.contains(baseFirmwareType) &&
            _availableCommands[baseFirmwareType].contains(baseVehicleType)) {
        // Available commands list already built
        return;
    }

    // Build new available commands list

    QMap<MAV_CMD, MissionCommandUIInfo*>& collapsedTree = _availableCommands[baseFirmwareType][baseVehicleType];

    // Any Firmware, Any Vehicle
    _collapseHierarchy(vehicle, _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC], collapsedTree);

    // Any Firmware, Specific Vehicle
    if (baseVehicleType != MAV_TYPE_GENERIC) {
        _collapseHierarchy(vehicle, _staticCommandTree[MAV_AUTOPILOT_GENERIC][baseVehicleType], collapsedTree);
    }

    // Known Firmware, Any Vehicle
    if (baseFirmwareType != MAV_AUTOPILOT_GENERIC) {
        _collapseHierarchy(vehicle, _staticCommandTree[baseFirmwareType][MAV_TYPE_GENERIC], collapsedTree);

        // Known Firmware, Specific Vehicle
        if (baseVehicleType != MAV_TYPE_GENERIC) {
            _collapseHierarchy(vehicle, _staticCommandTree[baseFirmwareType][baseVehicleType], collapsedTree);
        }
    }

    // Build category list
    QMapIterator<MAV_CMD, MissionCommandUIInfo*> iter(collapsedTree);
    while (iter.hasNext()) {
        iter.next();
        QString newCategory = iter.value()->category();
        if (!_availableCategories[baseFirmwareType][baseVehicleType].contains(newCategory)) {
            _availableCategories[baseFirmwareType][baseVehicleType].append(newCategory);
        }
    }
}

QStringList MissionCommandTree::_availableCategoriesForVehicle(Vehicle* vehicle)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
    _buildAvailableCommands(vehicle);

    return _availableCategories[baseFirmwareType][baseVehicleType];
}

QString MissionCommandTree::friendlyName(MAV_CMD command)
{
    MissionCommandList *    commandList =   _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC];
    MissionCommandUIInfo*   uiInfo =        commandList->getUIInfo(command);

    if (uiInfo) {
        return uiInfo->friendlyName();
    } else {
        return QString("MAV_CMD(%1)").arg((int)command);
    }
}

QString MissionCommandTree::rawName(MAV_CMD command)
{
    MissionCommandList *    commandList =   _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC];
    MissionCommandUIInfo*   uiInfo =        commandList->getUIInfo(command);

    if (uiInfo) {
        return uiInfo->rawName();
    } else {
        return QString("MAV_CMD(%1)").arg((int)command);
    }
}

const QList<MAV_CMD>& MissionCommandTree::allCommandIds(void) const
{
    return _staticCommandTree[MAV_AUTOPILOT_GENERIC][MAV_TYPE_GENERIC]->commandIds();
}

const MissionCommandUIInfo* MissionCommandTree::getUIInfo(Vehicle* vehicle, MAV_CMD command)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
    _buildAvailableCommands(vehicle);

    const QMap<MAV_CMD, MissionCommandUIInfo*>& infoMap = _availableCommands[baseFirmwareType][baseVehicleType];
    if (infoMap.contains(command)) {
        return infoMap[command];
    } else {
        return NULL;
    }
}

QVariantList MissionCommandTree::getCommandsForCategory(Vehicle* vehicle, const QString& category)
{
    MAV_AUTOPILOT   baseFirmwareType;
    MAV_TYPE        baseVehicleType;

    _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
    _buildAvailableCommands(vehicle);

    QVariantList list;
    QMap<MAV_CMD, MissionCommandUIInfo*> commandMap = _availableCommands[baseFirmwareType][baseVehicleType];
    foreach (MAV_CMD command, commandMap.keys()) {
        MissionCommandUIInfo* uiInfo = commandMap[command];
        if (uiInfo->category() == category) {
            list.append(QVariant::fromValue(uiInfo));
        }
    }

    return list;
}

void MissionCommandTree::_baseVehicleInfo(Vehicle* vehicle, MAV_AUTOPILOT& baseFirmwareType, MAV_TYPE& baseVehicleType) const
{
    if (vehicle) {
        baseFirmwareType = _baseFirmwareType(vehicle->firmwareType());
        baseVehicleType = _baseVehicleType(vehicle->vehicleType());
    } else {
        // No Vehicle means offline editing
        baseFirmwareType = _baseFirmwareType((MAV_AUTOPILOT)QGroundControlQmlGlobal::offlineEditingFirmwareType()->rawValue().toInt());
        baseVehicleType = _baseVehicleType((MAV_TYPE)QGroundControlQmlGlobal::offlineEditingVehicleType()->rawValue().toInt());
    }
}
<|endoftext|>


<commit_before>#include "connection.h"

namespace trikTelemetry {

Connection::Connection(trikControl::Brick &brick)
	: trikKernel::Connection(trikKernel::Protocol::messageLength)
	, mBrick(brick)
{
}

void Connection::processData(QByteArray const &data)
{
	QString command(data);

	QString portsRequested("ports");
	QString dataRequested("data");

	QString answer;

	if (command.startsWith(dataRequested)) {
		answer = "data:";
		answer += "analog:";
		for (QString port : mBrick.sensorPorts(trikControl::Sensor::analogSensor)) {
			answer += port + "=" + QString::number(mBrick.sensor(port)->read()) + ":";
			answer += QString::number(mBrick.sensor(port)->readRawData()) + ",";
		}
		answer[answer.length() - 1] = ';';
		answer += "digital:";
		for (QString port : mBrick.sensorPorts(trikControl::Sensor::digitalSensor)) {
			answer += port + "=" + QString::number(mBrick.sensor(port)->read()) + ":";
			answer += QString::number(mBrick.sensor(port)->readRawData()) + ",";
		}
		answer[answer.length() - 1] = ';';
		answer += "special:";
		for (QString port : mBrick.sensorPorts(trikControl::Sensor::specialSensor)) {
			answer += port + "=" + QString::number(mBrick.sensor(port)->read()) + ":";
			answer += QString::number(mBrick.sensor(port)->readRawData()) + ",";
		}
		answer[answer.length() - 1] = ';';
		answer += "accelerometer:" + serializeVector(mBrick.accelerometer()->read()) + ";";
		answer += "gyroscope:" + serializeVector(mBrick.gyroscope()->read());
	} else if (command.startsWith(portsRequested)) {
		answer = "ports:";
		answer += "analog:" + mBrick.sensorPorts(trikControl::Sensor::analogSensor).join(",") + ";";
		answer += "digital:" + mBrick.sensorPorts(trikControl::Sensor::digitalSensor).join(",") + ";";
		answer += "special:" + mBrick.sensorPorts(trikControl::Sensor::specialSensor).join(",");
	}

	send(answer.toUtf8());
}

QString Connection::serializeVector(QVector<int> const &vector) {
	QString result = "(";
	for (int coord : vector) {
		result += QString::number(coord) + ",";
	}

	result[result.length() - 1] = ')';
	return result;
}

}
<commit_msg>protocol is expanded<commit_after>#include "connection.h"

namespace trikTelemetry {

Connection::Connection(trikControl::Brick &brick)
	: trikKernel::Connection(trikKernel::Protocol::messageLength)
	, mBrick(brick)
{
}

void Connection::processData(QByteArray const &data)
{
	QString command(data);

	QString portsRequested("ports");
	QString dataRequested("data");
	QString singleSensorRequested("sensor:");
	QString accelerometerRequested("AccelerometerPort");
	QString gyroscopeRequested("GyroscopePort");

	QString answer;

	if (command.startsWith(dataRequested)) {
		answer = "data:";
		answer += "analog:";
		for (QString port : mBrick.sensorPorts(trikControl::Sensor::analogSensor)) {
			answer += port + "=" + QString::number(mBrick.sensor(port)->read()) + ":";
			answer += QString::number(mBrick.sensor(port)->readRawData()) + ",";
		}
		answer[answer.length() - 1] = ';';
		answer += "digital:";
		for (QString port : mBrick.sensorPorts(trikControl::Sensor::digitalSensor)) {
			answer += port + "=" + QString::number(mBrick.sensor(port)->read()) + ":";
			answer += QString::number(mBrick.sensor(port)->readRawData()) + ",";
		}
		answer[answer.length() - 1] = ';';
		answer += "special:";
		for (QString port : mBrick.sensorPorts(trikControl::Sensor::specialSensor)) {
			answer += port + "=" + QString::number(mBrick.sensor(port)->read()) + ":";
			answer += QString::number(mBrick.sensor(port)->readRawData()) + ",";
		}
		answer[answer.length() - 1] = ';';
		answer += "accelerometer:" + serializeVector(mBrick.accelerometer()->read()) + ";";
		answer += "gyroscope:" + serializeVector(mBrick.gyroscope()->read());
	} else if (command.startsWith(portsRequested)) {
		answer = "ports:";
		answer += "analog:" + mBrick.sensorPorts(trikControl::Sensor::analogSensor).join(",") + ";";
		answer += "digital:" + mBrick.sensorPorts(trikControl::Sensor::digitalSensor).join(",") + ";";
		answer += "special:" + mBrick.sensorPorts(trikControl::Sensor::specialSensor).join(",");
	} else if (command.startsWith(singleSensorRequested)) {
		answer = command + ":";
		command.remove(0, singleSensorRequested.length());
		if (command.startsWith(accelerometerRequested)) {
			int dimension = command.at(command.length() - 1).toAscii() - 'X';
			answer += QString::number(mBrick.accelerometer()->read()[dimension]);
		} else if (command.startsWith(gyroscopeRequested)) {
			int dimension = command.at(command.length() - 1).toAscii() - 'X';
			answer += QString::number(mBrick.gyroscope()->read()[dimension]);
		} else if (mBrick.sensorPorts(trikControl::Sensor::analogSensor).contains(command)
				|| mBrick.sensorPorts(trikControl::Sensor::digitalSensor).contains(command)
				|| mBrick.sensorPorts(trikControl::Sensor::specialSensor).contains(command)){
			answer += QString::number(mBrick.sensor(command)->read());
		} else if (mBrick.encoderPorts().contains(command)) {
			answer += QString::number(mBrick.encoder(command)->read());
		}
	}

	send(answer.toUtf8());
}

QString Connection::serializeVector(QVector<int> const &vector) {
	QString result = "(";
	for (int coord : vector) {
		result += QString::number(coord) + ",";
	}

	result[result.length() - 1] = ')';
	return result;
}

}
<|endoftext|>