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

  Program:   Visualization Toolkit
  Module:    vtkThreshold.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-1996 Ken Martin, Will Schroeder, Bill Lorensen.

This software is copyrighted by Ken Martin, Will Schroeder and Bill Lorensen.
The following terms apply to all files associated with the software unless
explicitly disclaimed in individual files. This copyright specifically does
not apply to the related textbook "The Visualization Toolkit" ISBN
013199837-4 published by Prentice Hall which is covered by its own copyright.

The authors hereby grant permission to use, copy, and distribute this
software and its documentation for any purpose, provided that existing
copyright notices are retained in all copies and that this notice is included
verbatim in any distributions. Additionally, the authors grant permission to
modify this software and its documentation for any purpose, provided that
such modifications are not distributed without the explicit consent of the
authors and that existing copyright notices are retained in all copies. Some
of the algorithms implemented by this software are patented, observe all
applicable patent law.

IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY FOR
DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY DERIVATIVES THEREOF,
EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE IS PROVIDED ON AN
"AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO PROVIDE
MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

=========================================================================*/
#include "vtkThreshold.h"

// Construct with lower threshold=0, upper threshold=1, and threshold
// function=upper AllScalars=1.
vtkThreshold::vtkThreshold()
{
  this->LowerThreshold = 0.0;
  this->UpperThreshold = 1.0;
  this->AllScalars = 1;
  this->ThresholdFunction = &vtkThreshold::Upper;
}

// Description:
// Criterion is cells whose scalars are less or equal to lower threshold.
void vtkThreshold::ThresholdByLower(float lower)
{
  if ( this->LowerThreshold != lower )
    {
    this->LowerThreshold = lower;
    this->ThresholdFunction = &vtkThreshold::Lower;
    this->Modified();
    }
}

// Description:
// Criterion is cells whose scalars are greater or equal to upper threshold.
void vtkThreshold::ThresholdByUpper(float upper)
{
  if ( this->UpperThreshold != upper )
    {
    this->UpperThreshold = upper;
    this->ThresholdFunction = &vtkThreshold::Upper;
    this->Modified();
    }
}

// Description:
// Criterion is cells whose scalars are between lower and upper thresholds.
void vtkThreshold::ThresholdBetween(float lower, float upper)
{
  if ( this->LowerThreshold != lower || this->UpperThreshold != upper )
    {
    this->LowerThreshold = lower;
    this->UpperThreshold = upper;
    this->ThresholdFunction = &vtkThreshold::Between;
    this->Modified();
    }
}

void vtkThreshold::Execute()
{
  int cellId;
  vtkIdList *cellPts, *pointMap;
  vtkIdList *newCellPts = new vtkIdList;
  vtkScalars *inScalars;
  vtkFloatScalars *cellScalars = new vtkFloatScalars;
  vtkCell *cell;
  vtkFloatPoints *newPoints;
  vtkPointData *pd, *outPD;
  int i, ptId, newId, numPts, numCellPts;
  float *x;
  vtkUnstructuredGrid *output= this->GetOutput();
  int keepCell;

  vtkDebugMacro(<< "Executing threshold filter");

  if ( ! (inScalars = this->Input->GetPointData()->GetScalars()) )
    {
    vtkErrorMacro(<<"No scalar data to threshold");
    return;
    }

  numPts = this->Input->GetNumberOfPoints();

  output->Allocate(this->Input->GetNumberOfCells());
  newPoints = new vtkFloatPoints(numPts);
  pd = this->Input->GetPointData();
  outPD = output->GetPointData();
  outPD->CopyAllocate(pd);

  pointMap = new vtkIdList(numPts); // maps old point ids into new
  for (i=0; i < numPts; i++) pointMap->SetId(i,-1);

  // Check that the scalars of each cell satisfy the threshold criterion
  for (cellId=0; cellId < this->Input->GetNumberOfCells(); cellId++)
    {
    cell = this->Input->GetCell(cellId);
    cellPts = cell->GetPointIds();
    inScalars->GetScalars(*cellPts,*cellScalars);
    numCellPts = cell->GetNumberOfPoints();

    if (this->AllScalars)
      {
      keepCell = 1;
      for ( i=0; keepCell && (i < numCellPts); i++)
	{
	ptId = cellPts->GetId(i);
	keepCell =
	  (this->*(this->ThresholdFunction))(cellScalars->GetScalar(ptId));
	}
      }
    else
      {
      keepCell = 0;
      for ( i=0; (!keepCell) && (i < numCellPts); i++)
	{
	ptId = cellPts->GetId(i);
	keepCell =
	  (this->*(this->ThresholdFunction))(cellScalars->GetScalar(ptId));
	}
      }

    if ( keepCell ) // satisfied thresholding
      {
      for (i=0; i < numCellPts; i++)
        {
        ptId = cellPts->GetId(i);
        if ( (newId = pointMap->GetId(ptId)) < 0 )
          {
          x = this->Input->GetPoint(ptId);
          newId = newPoints->InsertNextPoint(x);
          pointMap->SetId(ptId,newId);
          outPD->CopyData(pd,ptId,newId);
          }
        newCellPts->InsertId(i,newId);
        }
      output->InsertNextCell(cell->GetCellType(),*newCellPts);
      newCellPts->Reset();
      } // satisfied thresholding
    } // for all cells

  vtkDebugMacro(<< "Extracted " << output->GetNumberOfCells()
               << " number of cells.");

  // now clean up / update ourselves
  pointMap->Delete();
  newCellPts->Delete();

  output->SetPoints(newPoints);
  newPoints->Delete();

  output->Squeeze();
  cellScalars->Delete();
}

void vtkThreshold::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkDataSetToUnstructuredGridFilter::PrintSelf(os,indent);

  os << indent << "All Scalars: " << this->AllScalars << "\n";;
  if ( this->ThresholdFunction == &vtkThreshold::Upper )
    os << indent << "Threshold By Upper\n";

  else if ( this->ThresholdFunction == &vtkThreshold::Lower )
    os << indent << "Threshold By Lower\n";

  else if ( this->ThresholdFunction == &vtkThreshold::Between )
    os << indent << "Threshold Between\n";

  os << indent << "Lower Threshold: " << this->LowerThreshold << "\n";;
  os << indent << "Upper Threshold: " << this->UpperThreshold << "\n";;
}
<commit_msg>ENH: Fixed bug - accessing wrong scalars.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkThreshold.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 1993-1996 Ken Martin, Will Schroeder, Bill Lorensen.

This software is copyrighted by Ken Martin, Will Schroeder and Bill Lorensen.
The following terms apply to all files associated with the software unless
explicitly disclaimed in individual files. This copyright specifically does
not apply to the related textbook "The Visualization Toolkit" ISBN
013199837-4 published by Prentice Hall which is covered by its own copyright.

The authors hereby grant permission to use, copy, and distribute this
software and its documentation for any purpose, provided that existing
copyright notices are retained in all copies and that this notice is included
verbatim in any distributions. Additionally, the authors grant permission to
modify this software and its documentation for any purpose, provided that
such modifications are not distributed without the explicit consent of the
authors and that existing copyright notices are retained in all copies. Some
of the algorithms implemented by this software are patented, observe all
applicable patent law.

IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY FOR
DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY DERIVATIVES THEREOF,
EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE IS PROVIDED ON AN
"AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO PROVIDE
MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

=========================================================================*/
#include "vtkThreshold.h"

// Construct with lower threshold=0, upper threshold=1, and threshold
// function=upper AllScalars=1.
vtkThreshold::vtkThreshold()
{
  this->LowerThreshold = 0.0;
  this->UpperThreshold = 1.0;
  this->AllScalars = 1;
  this->ThresholdFunction = &vtkThreshold::Upper;
}

// Description:
// Criterion is cells whose scalars are less or equal to lower threshold.
void vtkThreshold::ThresholdByLower(float lower)
{
  if ( this->LowerThreshold != lower )
    {
    this->LowerThreshold = lower;
    this->ThresholdFunction = &vtkThreshold::Lower;
    this->Modified();
    }
}

// Description:
// Criterion is cells whose scalars are greater or equal to upper threshold.
void vtkThreshold::ThresholdByUpper(float upper)
{
  if ( this->UpperThreshold != upper )
    {
    this->UpperThreshold = upper;
    this->ThresholdFunction = &vtkThreshold::Upper;
    this->Modified();
    }
}

// Description:
// Criterion is cells whose scalars are between lower and upper thresholds.
void vtkThreshold::ThresholdBetween(float lower, float upper)
{
  if ( this->LowerThreshold != lower || this->UpperThreshold != upper )
    {
    this->LowerThreshold = lower;
    this->UpperThreshold = upper;
    this->ThresholdFunction = &vtkThreshold::Between;
    this->Modified();
    }
}

void vtkThreshold::Execute()
{
  int cellId;
  vtkIdList *cellPts, *pointMap;
  vtkIdList *newCellPts = new vtkIdList;
  vtkScalars *inScalars;
  vtkCell *cell;
  vtkFloatPoints *newPoints;
  vtkPointData *pd, *outPD;
  int i, ptId, newId, numPts, numCellPts;
  float *x;
  vtkUnstructuredGrid *output= this->GetOutput();
  int keepCell;

  vtkDebugMacro(<< "Executing threshold filter");

  if ( ! (inScalars = this->Input->GetPointData()->GetScalars()) )
    {
    vtkErrorMacro(<<"No scalar data to threshold");
    return;
    }

  numPts = this->Input->GetNumberOfPoints();

  output->Allocate(this->Input->GetNumberOfCells());
  newPoints = new vtkFloatPoints(numPts);
  pd = this->Input->GetPointData();
  outPD = output->GetPointData();
  outPD->CopyAllocate(pd);

  pointMap = new vtkIdList(numPts); // maps old point ids into new
  for (i=0; i < numPts; i++) pointMap->SetId(i,-1);

  // Check that the scalars of each cell satisfy the threshold criterion
  for (cellId=0; cellId < this->Input->GetNumberOfCells(); cellId++)
    {
    cell = this->Input->GetCell(cellId);
    cellPts = cell->GetPointIds();
    numCellPts = cell->GetNumberOfPoints();

    if (this->AllScalars)
      {
      keepCell = 1;
      for ( i=0; keepCell && (i < numCellPts); i++)
	{
	ptId = cellPts->GetId(i);
	keepCell =
	  (this->*(this->ThresholdFunction))(inScalars->GetScalar(ptId));
	}
      }
    else
      {
      keepCell = 0;
      for ( i=0; (!keepCell) && (i < numCellPts); i++)
	{
	ptId = cellPts->GetId(i);
	keepCell =
	  (this->*(this->ThresholdFunction))(inScalars->GetScalar(ptId));
	}
      }

    if ( keepCell ) // satisfied thresholding
      {
      for (i=0; i < numCellPts; i++)
        {
        ptId = cellPts->GetId(i);
        if ( (newId = pointMap->GetId(ptId)) < 0 )
          {
          x = this->Input->GetPoint(ptId);
          newId = newPoints->InsertNextPoint(x);
          pointMap->SetId(ptId,newId);
          outPD->CopyData(pd,ptId,newId);
          }
        newCellPts->InsertId(i,newId);
        }
      output->InsertNextCell(cell->GetCellType(),*newCellPts);
      newCellPts->Reset();
      } // satisfied thresholding
    } // for all cells

  vtkDebugMacro(<< "Extracted " << output->GetNumberOfCells()
                << " number of cells.");

  // now clean up / update ourselves
  pointMap->Delete();
  newCellPts->Delete();

  output->SetPoints(newPoints);
  newPoints->Delete();

  output->Squeeze();
}

void vtkThreshold::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkDataSetToUnstructuredGridFilter::PrintSelf(os,indent);

  os << indent << "All Scalars: " << this->AllScalars << "\n";;
  if ( this->ThresholdFunction == &vtkThreshold::Upper )
    os << indent << "Threshold By Upper\n";

  else if ( this->ThresholdFunction == &vtkThreshold::Lower )
    os << indent << "Threshold By Lower\n";

  else if ( this->ThresholdFunction == &vtkThreshold::Between )
    os << indent << "Threshold Between\n";

  os << indent << "Lower Threshold: " << this->LowerThreshold << "\n";;
  os << indent << "Upper Threshold: " << this->UpperThreshold << "\n";;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2018, Ford Motor Company
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided with the
 * distribution.
 *
 * Neither the name of the Ford Motor Company nor the names of its contributors
 * may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "sdl_rpc_plugin/commands/mobile/send_haptic_data_request.h"
#include "interfaces/MOBILE_API.h"

namespace sdl_rpc_plugin {
using namespace application_manager;

namespace commands {

SDL_CREATE_LOG_VARIABLE("Commands")

namespace custom_str = utils::custom_string;

SendHapticDataRequest::SendHapticDataRequest(
    const application_manager::commands::MessageSharedPtr& message,
    ApplicationManager& application_manager,
    app_mngr::rpc_service::RPCService& rpc_service,
    app_mngr::HMICapabilities& hmi_capabilities,
    policy::PolicyHandlerInterface& policy_handler)
    : CommandRequestImpl(message,
                         application_manager,
                         rpc_service,
                         hmi_capabilities,
                         policy_handler) {}

SendHapticDataRequest::~SendHapticDataRequest() {}

void SendHapticDataRequest::Run() {
  SDL_LOG_AUTO_TRACE();

  smart_objects::SmartObject& msg_params = (*message_)[strings::msg_params];

  ApplicationSharedPtr app = application_manager_.application(connection_key());

  if (!app) {
    SDL_LOG_ERROR("Application is not registered");
    SendResponse(false, mobile_apis::Result::APPLICATION_NOT_REGISTERED);
    return;
  }

  if (app->is_navi() || app->mobile_projection_enabled()) {
    SendHMIRequest(hmi_apis::FunctionID::UI_SendHapticData, &msg_params, true);
  } else {
    SendResponse(false,
                 mobile_apis::Result::DISALLOWED,
                 "Application is not of type Navigation or Mobile Projection");
  }
}

void SendHapticDataRequest::on_event(const event_engine::Event& event) {
  SDL_LOG_AUTO_TRACE();
  using namespace helpers;

  const smart_objects::SmartObject& message = event.smart_object();

  switch (event.id()) {
    case hmi_apis::FunctionID::UI_SendHapticData: {
      mobile_apis::Result::eType result_code =
          GetMobileResultCode(static_cast<hmi_apis::Common_Result::eType>(
              message[strings::params][hmi_response::code].asUInt()));

      const bool result = Compare<mobile_api::Result::eType, EQ, ONE>(
          result_code,
          mobile_api::Result::SUCCESS,
          mobile_api::Result::WARNINGS);

      SendResponse(result, result_code, NULL, &(message[strings::msg_params]));
      break;
    }
    default: {
      SDL_LOG_ERROR("Received unknown event " << event.id());
      return;
    }
  }
}

}  // namespace commands

}  // namespace sdl_rpc_plugin
<commit_msg>add appID param to SendHapticData HMI request (#3646)<commit_after>/*
 * Copyright (c) 2018, Ford Motor Company
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided with the
 * distribution.
 *
 * Neither the name of the Ford Motor Company nor the names of its contributors
 * may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "sdl_rpc_plugin/commands/mobile/send_haptic_data_request.h"
#include "interfaces/MOBILE_API.h"

namespace sdl_rpc_plugin {
using namespace application_manager;

namespace commands {

SDL_CREATE_LOG_VARIABLE("Commands")

namespace custom_str = utils::custom_string;

SendHapticDataRequest::SendHapticDataRequest(
    const application_manager::commands::MessageSharedPtr& message,
    ApplicationManager& application_manager,
    app_mngr::rpc_service::RPCService& rpc_service,
    app_mngr::HMICapabilities& hmi_capabilities,
    policy::PolicyHandlerInterface& policy_handler)
    : CommandRequestImpl(message,
                         application_manager,
                         rpc_service,
                         hmi_capabilities,
                         policy_handler) {}

SendHapticDataRequest::~SendHapticDataRequest() {}

void SendHapticDataRequest::Run() {
  SDL_LOG_AUTO_TRACE();

  smart_objects::SmartObject& msg_params = (*message_)[strings::msg_params];

  ApplicationSharedPtr app = application_manager_.application(connection_key());

  if (!app) {
    SDL_LOG_ERROR("Application is not registered");
    SendResponse(false, mobile_apis::Result::APPLICATION_NOT_REGISTERED);
    return;
  }

  if (app->is_navi() || app->mobile_projection_enabled()) {
    msg_params[strings::app_id] = connection_key();
    SendHMIRequest(hmi_apis::FunctionID::UI_SendHapticData, &msg_params, true);
  } else {
    SendResponse(false,
                 mobile_apis::Result::DISALLOWED,
                 "Application is not of type Navigation or Mobile Projection");
  }
}

void SendHapticDataRequest::on_event(const event_engine::Event& event) {
  SDL_LOG_AUTO_TRACE();
  using namespace helpers;

  const smart_objects::SmartObject& message = event.smart_object();

  switch (event.id()) {
    case hmi_apis::FunctionID::UI_SendHapticData: {
      mobile_apis::Result::eType result_code =
          GetMobileResultCode(static_cast<hmi_apis::Common_Result::eType>(
              message[strings::params][hmi_response::code].asUInt()));

      const bool result = Compare<mobile_api::Result::eType, EQ, ONE>(
          result_code,
          mobile_api::Result::SUCCESS,
          mobile_api::Result::WARNINGS);

      SendResponse(result, result_code, NULL, &(message[strings::msg_params]));
      break;
    }
    default: {
      SDL_LOG_ERROR("Received unknown event " << event.id());
      return;
    }
  }
}

}  // namespace commands

}  // namespace sdl_rpc_plugin
<|endoftext|>


<commit_before>//$Id$

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S.
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software.
//
//Pursuant to DoD Directive 523024
//
// DISTRIBUTION STATEMENT A: This software has been approved for public
//                           release, distribution is unlimited.
//
//=============================================================================

#include <string>
#include <sstream>

#include <list>
#include <map>
#include <set>

#include "DayTime.hpp"
#include "GPSWeekSecond.hpp"
#include "TimeConstants.hpp"
#include "Exception.hpp"
#include "CommandOption.hpp"
#include "CommandOptionParser.hpp"
#include "DeviceStream.hpp"

#include "StringUtils.hpp"
#include "InOutFramework.hpp"
#include "ObsUtils.hpp"

#include "MDPStream.hpp"
#include "MDPNavSubframe.hpp"
#include "MDPObsEpoch.hpp"
#include "MDPSelftestStatus.hpp"

using namespace std;
using namespace gpstk;
using namespace gpstk::StringUtils;

class MDPEdit : public InOutFramework<MDPStream, MDPStream>
{
public:
   MDPEdit(const string& applName)
      throw()
      : InOutFramework<MDPStream, MDPStream>(
         applName, "Edits a MDP file based on various criteria.")
   {}

   bool initialize(int argc, char *argv[]) throw()
   {
      // set up options. note that nothing is required
      CommandOptionWithAnyArg startOpt('s',"start","Throw out data before"
         " this time. Format as string: \"yyyy ddd HH:MM:SS\" ");
      CommandOptionWithAnyArg endOpt('e',"end","Throw out data after this"
         " time. Format as string: \"yyyy ddd HH:MM:SS\" ");
      CommandOptionWithNumberArg prnOpt('p',"PRN","Throw out obs data from"
         " this PRN. Repeat option for mutiple SVs.");
      CommandOptionWithNumberArg recordStartOpt('\0',"record-start","Throw"
         " out data before this record number.");
      CommandOptionWithNumberArg recordEndOpt('\0',"record-end","Throw out"
         " data after this record number.");
      CommandOptionNoArg obsOpt('O',"no-obs","Remove all obs messages.");
      CommandOptionNoArg navOpt('N',"no-nav","Remove all nav messages.");
      CommandOptionNoArg pvtOpt('P',"no-pvt","Remove all pvt messages.");
      CommandOptionNoArg stsOpt('S',"no-sts","Remove all self test "
                                    "status messages.");

      if (!InOutFramework<MDPStream, MDPStream>::initialize(argc,argv))
         return false;

      // get any PRNs to toss
      for (int index = 0; index < prnOpt.getCount(); index++ )
      {
         int prn = asInt(prnOpt.getValue()[index]);

         if ((prn < 1)||(prn > gpstk::MAX_PRN))
         {
            cout << "\n You entered an invalid PRN."
                 << "\n Exiting.\n\n";
            return false;
         }
         else
         {
            prnSetToToss.insert(prn);
            if (debugLevel || verboseLevel)
               cout << "Throwing out data from PRN " << prn << endl;
         }
      }

      // get any time limits
      if (startOpt.getCount())
      {
         tStart.setToString(startOpt.getValue().front().c_str(),
                            "%Y %j %H:%M:%S");
         if (debugLevel)
            cout << "Throwing out data before " << tStart << endl;
      }
      else
      {
         tStart = DayTime::BEGINNING_OF_TIME;
         if (debugLevel || verboseLevel)
            cout << "No start time given.\n";
      }

      if (endOpt.getCount())
      {
         tEnd.setToString(endOpt.getValue().front().c_str(),"%Y %j %H:%M:%S");
         if (debugLevel || verboseLevel)
            cout << "Throwing out data after  " << tEnd << endl;
      }
      else
      {
         tEnd = DayTime::END_OF_TIME;
         if (debugLevel || verboseLevel)
            cout << "No end time given.\n";
      }

      recordStart = recordEnd = 0;
      if (recordStartOpt.getCount())
      {
         recordStart = asInt(recordStartOpt.getValue().front());
         if (debugLevel || verboseLevel)
            cout << "Throwing out data before record number "
                 << recordStart << endl;
      }
      if (recordEndOpt.getCount())
      {
         recordEnd = asInt(recordEndOpt.getValue().front());
         if (debugLevel || verboseLevel)
            cout << "Throwing out data after record number "
                 << recordEnd << endl;
      }

      // see if any message types should be removed
      noObs = noNav = noPvt = noSts = false;
      if (obsOpt.getCount())
      {
         noObs = true;
         if (debugLevel || verboseLevel)
            cout << "Removing obs messages.\n";
      }
      if (navOpt.getCount())
      {
         noNav = true;
         if (debugLevel || verboseLevel)
            cout << "Removing nav messages.\n";
      }
      if (pvtOpt.getCount())
      {
         noPvt = true;
         if (debugLevel || verboseLevel)
            cout << "Removing pvt messages.\n";
      }
      if (stsOpt.getCount())
      {
         noSts = true;
         if (debugLevel || verboseLevel)
            cout << "Removing self test status messages.\n";
      }

      return true;
   }

protected:
   virtual void spinUp()
   {}

   virtual void process()
   {
      msgCount=0;

      die = false;

      DayTime currEpoch;
      MDPEpoch oe;
      uint16_t fc=0;

      while (!input.eof())
      {
         input >> header;

         if (!input)
            break;
         else if (header.time > tEnd)
            return;
         else if (recordEnd && (input.recordNumber > recordEnd))
	    return;
	 else if (header.time < tStart)
	    continue;
         else if (recordStart && (input.recordNumber < recordStart))
            continue;

         msgCount++;

         if (msgCount == 1)
         {
            fc = header.freshnessCount;
            if (debugLevel)
               cout << "First message at " << header.time << endl;
         }

         if (verboseLevel > 4 || debugLevel > 3)
            cout << "Record: "    << input.recordNumber
                 << ", message: " << msgCount << ":" << endl;

         switch (input.header.id)
         {
            case gpstk::MDPObsEpoch::myId:
               if (!noObs)
               {
                  MDPObsEpoch obs;
                  input >> obs;
                  if (obs)
                  {
                     if (prnSetToToss.size())
                     {
                        // If we have moved on to a new time, output the previous epoch
                        if (currEpoch != obs.time)
                        {
                           if (!oe.empty())
                           {
                              MDPEpoch::iterator i;
                              for (i=oe.begin(); i != oe.end(); i++)
                              {
                                 i->second.numSVs = oe.size();
                                 i->second.freshnessCount = fc++;
                                 output << i->second;
                              }
                           }
                           currEpoch = obs.time;
                           oe.clear();
                        }
                        if (!prnSetToToss.count(obs.prn))
                        {
                           oe.insert(pair<int, MDPObsEpoch>(obs.prn, obs));
                           if (debugLevel > 2)
                           {
                              cout << "  Writing obs message:\n";
                              obs.dump(cout);
                           }
                        }
                        else
                        {
                           if (debugLevel > 2)
                              cout << "  Not writing obs message for PRN "
                                   << obs.prn << endl;
                        }
                     }
                     else
                     {
                        if (debugLevel > 2)
                        {
                           cout << "  Writing obs message:\n";
                           obs.dump(cout);
                        }
                        obs.freshnessCount = fc++;
                        output << obs;
                     }
                  }
                  else
                  {
                     if (debugLevel > 2)
                     {
                        cout << "  Tossing obs message due to a bad read:\n";
                        obs.dump(cout);
                     }
                  }
               }
               else
               {
                  if (debugLevel > 3)
                  {
                     cout << "  Ignoring obs message from record "
                          << input.recordNumber << endl;
                  }
               }
               break;

            case gpstk::MDPPVTSolution::myId:
               if (!noPvt)
               {
                  MDPPVTSolution pvt;
                  input  >> pvt;
                  if (pvt)
                  {
                     pvt.freshnessCount = fc++;
                     output << pvt;
                     if (debugLevel > 2)
                     {
                        cout << "  Writing pvt message:\n";
                        pvt.dump(cout);
                     }
                  }
                  else if (debugLevel > 2)
                  {
                     cout << "  Tossing pvt message:\n";
                     pvt.dump(cout);
                  }
               }
               else if (debugLevel > 3)
               {
                  cout << "  Ignoring pvt message from record "
                       << input.recordNumber << endl;
               }
               break;

            case gpstk::MDPNavSubframe::myId:
               if (!noNav)
               {
                  MDPNavSubframe nav;
                  input  >> nav;
                  if (nav)
                  {
                     nav.freshnessCount = fc++;
                     output << nav;
                     if (debugLevel > 2)
                     {
                        cout << "  Writing nav message:\n";
                        nav.dump(cout);
                     }
                  }
                  else if (debugLevel > 2)
                  {
                     cout << "  Tossing nav message:\n";
                     nav.dump(cout);
                  }
               }
               else if (debugLevel > 3)
               {
                  cout << "  Ignoring nav message from record "
                       << input.recordNumber << endl;
               }
               break;

            case gpstk::MDPSelftestStatus::myId:
               if (!noSts)
               {
                  gpstk::MDPSelftestStatus sts;
                  input  >> sts;
                  if (sts)
                  {
                     sts.freshnessCount = fc++;
                     output << sts;
                     if (debugLevel > 2)
                     {
                        cout << "  Writing self test status message:\n";
                        sts.dump(cout);
                     }
                  }
               }
               else if (debugLevel > 3)
               {
                  cout << "  Ignoring status message from record "
                       << input.recordNumber << endl;
               }
               break;

         } // switch (input.header.id)
      }    // while (!input.eof())

      timeToDie = true;

   }       // virtual void process()

   virtual void shutDown()
   {
      if (verboseLevel)
         cout << "Doneskies.\n";
   }

private:
   bool noObs, noNav, noPvt, noSts, die;
   DayTime tStart, tEnd;
   set<int> prnSetToToss;
   unsigned int recordStart, recordEnd;
   unsigned long msgCount, fcErrorCount;
   unsigned short firstFC, lastFC;
   MDPHeader header;

};

int main(int argc, char *argv[])
{
   MDPEdit crap(argv[0]);

   if (!crap.initialize(argc, argv))
      exit(0);

   crap.run();
}
<commit_msg>Removing unnecessary include<commit_after>//$Id$

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S.
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software.
//
//Pursuant to DoD Directive 523024
//
// DISTRIBUTION STATEMENT A: This software has been approved for public
//                           release, distribution is unlimited.
//
//=============================================================================

#include <string>
#include <sstream>

#include <list>
#include <map>
#include <set>

#include "DayTime.hpp"
#include "GPSWeekSecond.hpp"
#include "TimeConstants.hpp"
#include "Exception.hpp"
#include "CommandOption.hpp"
#include "CommandOptionParser.hpp"

#include "StringUtils.hpp"
#include "InOutFramework.hpp"
#include "ObsUtils.hpp"

#include "MDPStream.hpp"
#include "MDPNavSubframe.hpp"
#include "MDPObsEpoch.hpp"
#include "MDPSelftestStatus.hpp"

using namespace std;
using namespace gpstk;
using namespace gpstk::StringUtils;

class MDPEdit : public InOutFramework<MDPStream, MDPStream>
{
public:
   MDPEdit(const string& applName)
      throw()
      : InOutFramework<MDPStream, MDPStream>(
         applName, "Edits a MDP file based on various criteria.")
   {}

   bool initialize(int argc, char *argv[]) throw()
   {
      // set up options. note that nothing is required
      CommandOptionWithAnyArg startOpt('s',"start","Throw out data before"
         " this time. Format as string: \"yyyy ddd HH:MM:SS\" ");
      CommandOptionWithAnyArg endOpt('e',"end","Throw out data after this"
         " time. Format as string: \"yyyy ddd HH:MM:SS\" ");
      CommandOptionWithNumberArg prnOpt('p',"PRN","Throw out obs data from"
         " this PRN. Repeat option for mutiple SVs.");
      CommandOptionWithNumberArg recordStartOpt('\0',"record-start","Throw"
         " out data before this record number.");
      CommandOptionWithNumberArg recordEndOpt('\0',"record-end","Throw out"
         " data after this record number.");
      CommandOptionNoArg obsOpt('O',"no-obs","Remove all obs messages.");
      CommandOptionNoArg navOpt('N',"no-nav","Remove all nav messages.");
      CommandOptionNoArg pvtOpt('P',"no-pvt","Remove all pvt messages.");
      CommandOptionNoArg stsOpt('S',"no-sts","Remove all self test "
                                    "status messages.");

      if (!InOutFramework<MDPStream, MDPStream>::initialize(argc,argv))
         return false;

      // get any PRNs to toss
      for (int index = 0; index < prnOpt.getCount(); index++ )
      {
         int prn = asInt(prnOpt.getValue()[index]);

         if ((prn < 1)||(prn > gpstk::MAX_PRN))
         {
            cout << "\n You entered an invalid PRN."
                 << "\n Exiting.\n\n";
            return false;
         }
         else
         {
            prnSetToToss.insert(prn);
            if (debugLevel || verboseLevel)
               cout << "Throwing out data from PRN " << prn << endl;
         }
      }

      // get any time limits
      if (startOpt.getCount())
      {
         tStart.setToString(startOpt.getValue().front().c_str(),
                            "%Y %j %H:%M:%S");
         if (debugLevel)
            cout << "Throwing out data before " << tStart << endl;
      }
      else
      {
         tStart = DayTime::BEGINNING_OF_TIME;
         if (debugLevel || verboseLevel)
            cout << "No start time given.\n";
      }

      if (endOpt.getCount())
      {
         tEnd.setToString(endOpt.getValue().front().c_str(),"%Y %j %H:%M:%S");
         if (debugLevel || verboseLevel)
            cout << "Throwing out data after  " << tEnd << endl;
      }
      else
      {
         tEnd = DayTime::END_OF_TIME;
         if (debugLevel || verboseLevel)
            cout << "No end time given.\n";
      }

      recordStart = recordEnd = 0;
      if (recordStartOpt.getCount())
      {
         recordStart = asInt(recordStartOpt.getValue().front());
         if (debugLevel || verboseLevel)
            cout << "Throwing out data before record number "
                 << recordStart << endl;
      }
      if (recordEndOpt.getCount())
      {
         recordEnd = asInt(recordEndOpt.getValue().front());
         if (debugLevel || verboseLevel)
            cout << "Throwing out data after record number "
                 << recordEnd << endl;
      }

      // see if any message types should be removed
      noObs = noNav = noPvt = noSts = false;
      if (obsOpt.getCount())
      {
         noObs = true;
         if (debugLevel || verboseLevel)
            cout << "Removing obs messages.\n";
      }
      if (navOpt.getCount())
      {
         noNav = true;
         if (debugLevel || verboseLevel)
            cout << "Removing nav messages.\n";
      }
      if (pvtOpt.getCount())
      {
         noPvt = true;
         if (debugLevel || verboseLevel)
            cout << "Removing pvt messages.\n";
      }
      if (stsOpt.getCount())
      {
         noSts = true;
         if (debugLevel || verboseLevel)
            cout << "Removing self test status messages.\n";
      }

      return true;
   }

protected:
   virtual void spinUp()
   {}

   virtual void process()
   {
      msgCount=0;

      die = false;

      DayTime currEpoch;
      MDPEpoch oe;
      uint16_t fc=0;

      while (!input.eof())
      {
         input >> header;

         if (!input)
            break;
         else if (header.time > tEnd)
            return;
         else if (recordEnd && (input.recordNumber > recordEnd))
	    return;
	 else if (header.time < tStart)
	    continue;
         else if (recordStart && (input.recordNumber < recordStart))
            continue;

         msgCount++;

         if (msgCount == 1)
         {
            fc = header.freshnessCount;
            if (debugLevel)
               cout << "First message at " << header.time << endl;
         }

         if (verboseLevel > 4 || debugLevel > 3)
            cout << "Record: "    << input.recordNumber
                 << ", message: " << msgCount << ":" << endl;

         switch (input.header.id)
         {
            case gpstk::MDPObsEpoch::myId:
               if (!noObs)
               {
                  MDPObsEpoch obs;
                  input >> obs;
                  if (obs)
                  {
                     if (prnSetToToss.size())
                     {
                        // If we have moved on to a new time, output the previous epoch
                        if (currEpoch != obs.time)
                        {
                           if (!oe.empty())
                           {
                              MDPEpoch::iterator i;
                              for (i=oe.begin(); i != oe.end(); i++)
                              {
                                 i->second.numSVs = oe.size();
                                 i->second.freshnessCount = fc++;
                                 output << i->second;
                              }
                           }
                           currEpoch = obs.time;
                           oe.clear();
                        }
                        if (!prnSetToToss.count(obs.prn))
                        {
                           oe.insert(pair<int, MDPObsEpoch>(obs.prn, obs));
                           if (debugLevel > 2)
                           {
                              cout << "  Writing obs message:\n";
                              obs.dump(cout);
                           }
                        }
                        else
                        {
                           if (debugLevel > 2)
                              cout << "  Not writing obs message for PRN "
                                   << obs.prn << endl;
                        }
                     }
                     else
                     {
                        if (debugLevel > 2)
                        {
                           cout << "  Writing obs message:\n";
                           obs.dump(cout);
                        }
                        obs.freshnessCount = fc++;
                        output << obs;
                     }
                  }
                  else
                  {
                     if (debugLevel > 2)
                     {
                        cout << "  Tossing obs message due to a bad read:\n";
                        obs.dump(cout);
                     }
                  }
               }
               else
               {
                  if (debugLevel > 3)
                  {
                     cout << "  Ignoring obs message from record "
                          << input.recordNumber << endl;
                  }
               }
               break;

            case gpstk::MDPPVTSolution::myId:
               if (!noPvt)
               {
                  MDPPVTSolution pvt;
                  input  >> pvt;
                  if (pvt)
                  {
                     pvt.freshnessCount = fc++;
                     output << pvt;
                     if (debugLevel > 2)
                     {
                        cout << "  Writing pvt message:\n";
                        pvt.dump(cout);
                     }
                  }
                  else if (debugLevel > 2)
                  {
                     cout << "  Tossing pvt message:\n";
                     pvt.dump(cout);
                  }
               }
               else if (debugLevel > 3)
               {
                  cout << "  Ignoring pvt message from record "
                       << input.recordNumber << endl;
               }
               break;

            case gpstk::MDPNavSubframe::myId:
               if (!noNav)
               {
                  MDPNavSubframe nav;
                  input  >> nav;
                  if (nav)
                  {
                     nav.freshnessCount = fc++;
                     output << nav;
                     if (debugLevel > 2)
                     {
                        cout << "  Writing nav message:\n";
                        nav.dump(cout);
                     }
                  }
                  else if (debugLevel > 2)
                  {
                     cout << "  Tossing nav message:\n";
                     nav.dump(cout);
                  }
               }
               else if (debugLevel > 3)
               {
                  cout << "  Ignoring nav message from record "
                       << input.recordNumber << endl;
               }
               break;

            case gpstk::MDPSelftestStatus::myId:
               if (!noSts)
               {
                  gpstk::MDPSelftestStatus sts;
                  input  >> sts;
                  if (sts)
                  {
                     sts.freshnessCount = fc++;
                     output << sts;
                     if (debugLevel > 2)
                     {
                        cout << "  Writing self test status message:\n";
                        sts.dump(cout);
                     }
                  }
               }
               else if (debugLevel > 3)
               {
                  cout << "  Ignoring status message from record "
                       << input.recordNumber << endl;
               }
               break;

         } // switch (input.header.id)
      }    // while (!input.eof())

      timeToDie = true;

   }       // virtual void process()

   virtual void shutDown()
   {
      if (verboseLevel)
         cout << "Doneskies.\n";
   }

private:
   bool noObs, noNav, noPvt, noSts, die;
   DayTime tStart, tEnd;
   set<int> prnSetToToss;
   unsigned int recordStart, recordEnd;
   unsigned long msgCount, fcErrorCount;
   unsigned short firstFC, lastFC;
   MDPHeader header;

};

int main(int argc, char *argv[])
{
   MDPEdit crap(argv[0]);

   if (!crap.initialize(argc, argv))
      exit(0);

   crap.run();
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 Cloudius Systems, Ltd.
 */

#include "tests/http_response_parser.hh"
#include "core/print.hh"
#include "core/reactor.hh"
#include "core/app-template.hh"
#include "core/future-util.hh"
#include "core/distributed.hh"
#include "core/semaphore.hh"
#include "core/future-util.hh"
#include <chrono>

template <typename... Args>
void http_debug(const char* fmt, Args&&... args) {
#if HTTP_DEBUG
    print(fmt, std::forward<Args>(args)...);
#endif
}

class http_client {
private:
    unsigned _duration;
    unsigned _conn_per_core;
    unsigned _reqs_per_conn;
    std::vector<connected_socket> _sockets;
    semaphore _conn_connected{0};
    semaphore _conn_finished{0};
    timer<> _run_timer;
    bool _timer_based;
    bool _timer_done{false};
    uint64_t _total_reqs{0};
public:
    http_client(unsigned duration, unsigned total_conn, unsigned reqs_per_conn)
        : _duration(duration)
        , _conn_per_core(total_conn / smp::count)
        , _reqs_per_conn(reqs_per_conn)
        , _run_timer([this] { _timer_done = true; })
        , _timer_based(reqs_per_conn == 0) {
    }

    class connection {
    private:
        connected_socket _fd;
        input_stream<char> _read_buf;
        output_stream<char> _write_buf;
        http_response_parser _parser;
        http_client* _http_client;
        uint64_t _nr_done{0};
    public:
        connection(connected_socket&& fd, http_client* client)
            : _fd(std::move(fd))
            , _read_buf(_fd.input())
            , _write_buf(_fd.output())
            , _http_client(client){
        }

        uint64_t nr_done() {
            return _nr_done;
        }

        future<> do_req() {
            return _write_buf.write("GET / HTTP/1.1\r\nHost: 127.0.0.1:10000\r\n\r\n").then([this] {
                return _write_buf.flush();
            }).then([this] {
                _parser.init();
                return _read_buf.consume(_parser).then([this] {
                    // Read HTTP response header first
                    if (_parser.eof()) {
                        return make_ready_future<>();
                    }
                    auto _rsp = _parser.get_parsed_response();
                    auto it = _rsp->_headers.find("Content-Length");
                    if (it == _rsp->_headers.end()) {
                        print("Error: HTTP response does not contain: Content-Length\n");
                        return make_ready_future<>();
                    }
                    auto content_len = std::stoi(it->second);
                    http_debug("Content-Length = %d\n", content_len);
                    // Read HTTP response body
                    return _read_buf.read_exactly(content_len).then([this] (temporary_buffer<char> buf) {
                        _nr_done++;
                        http_debug("%s\n", buf.get());
                        if (_http_client->done(_nr_done)) {
                            return make_ready_future();
                        } else {
                            return do_req();
                        }
                    });
                });
            });
        }
    };

    future<uint64_t> total_reqs() {
        print("Requests on cpu %2d: %ld\n", engine().cpu_id(), _total_reqs);
        return make_ready_future<uint64_t>(_total_reqs);
    }

    bool done(uint64_t nr_done) {
        if (_timer_based) {
            return _timer_done;
        } else {
            return nr_done >= _reqs_per_conn;
        }
    }

    future<> connect(ipv4_addr server_addr) {
        // Establish all the TCP connections first
        for (unsigned i = 0; i < _conn_per_core; i++) {
            engine().net().connect(make_ipv4_address(server_addr)).then([this] (connected_socket fd) {
                _sockets.push_back(std::move(fd));
                http_debug("Established connection %6d on cpu %3d\n", _conn_connected.current(), engine().cpu_id());
                _conn_connected.signal();
            });
        }
        return _conn_connected.wait(_conn_per_core);
    }

    future<> run() {
        // All connected, start HTTP request
        http_debug("Established all %6d tcp connections on cpu %3d\n", _conn_per_core, engine().cpu_id());
        if (_timer_based) {
            _run_timer.arm(std::chrono::seconds(_duration));
        }
        for (auto&& fd : _sockets) {
            auto conn = new connection(std::move(fd), this);
            conn->do_req().rescue([this, conn] (auto get_ex) {
                http_debug("Finished connection %6d on cpu %3d\n", _conn_finished.current(), engine().cpu_id());
                _total_reqs += conn->nr_done();
                _conn_finished.signal();
                delete conn;
                try {
                    get_ex();
                } catch (std::exception& ex) {
                    print("http request error: %s\n", ex.what());
                }
            });
        }

        // All finished
        return _conn_finished.wait(_conn_per_core);
    }
    future<> stop() {
        return make_ready_future();
    }
};

namespace bpo = boost::program_options;

int main(int ac, char** av) {
    app_template app;
    app.add_options()
        ("server,s", bpo::value<std::string>()->default_value("192.168.66.100:10000"), "Server address")
        ("conn,c", bpo::value<unsigned>()->default_value(100), "total connections")
        ("reqs,r", bpo::value<unsigned>()->default_value(0), "reqs per connection")
        ("duration,d", bpo::value<unsigned>()->default_value(10), "duration of the test in seconds)");

    return app.run(ac, av, [&app] {
        auto& config = app.configuration();
        auto server = config["server"].as<std::string>();
        auto reqs_per_conn = config["reqs"].as<unsigned>();
        auto total_conn= config["conn"].as<unsigned>();
        auto duration = config["duration"].as<unsigned>();

        if (total_conn % smp::count != 0) {
            print("Error: conn needs to be n * cpu_nr\n");
            return engine().exit(0);
        }

        auto http_clients = new distributed<http_client>;

        // Start http requests on all the cores
        auto started = std::chrono::high_resolution_clock::now();
        print("========== http_client ============\n");
        print("Server: %s\n", server);
        print("Connections: %u\n", total_conn);
        print("Requests/connection: %s\n", reqs_per_conn == 0 ? "dynamic (timer based)" : std::to_string(reqs_per_conn));
        http_clients->start(std::ref(duration), std::ref(total_conn), std::ref(reqs_per_conn)).then([http_clients, started, server] {
            return http_clients->invoke_on_all(&http_client::connect, ipv4_addr{server});
        }).then([http_clients] {
            return http_clients->invoke_on_all(&http_client::run);
        }).then([http_clients] {
            return http_clients->map_reduce(adder<uint64_t>(), &http_client::total_reqs);
        }).then([http_clients, started] (auto total_reqs) {
           // All the http requests are finished
           auto finished = std::chrono::high_resolution_clock::now();
           auto elapsed = finished - started;
           auto secs = static_cast<double>(elapsed.count() / 1000000000.0);
           print("Total cpus: %u\n", smp::count);
           print("Total requests: %u\n", total_reqs);
           print("Total time: %f\n", secs);
           print("Requests/sec: %f\n", static_cast<double>(total_reqs) / secs);
           print("==========     done     ============\n");
           http_clients->stop().then([http_clients] {
               // FIXME: If we call engine().exit(0) here to exit when
               // requests are done. The tcp connection will not be closed
               // properly, becasue we exit too earily and the FIN packets are
               // not exchanged.
                delete http_clients;
           });
        });
    });
}
<commit_msg>seawreck: fix include<commit_after>/*
 * Copyright (C) 2015 Cloudius Systems, Ltd.
 */

#include "apps/seawreck/http_response_parser.hh"
#include "core/print.hh"
#include "core/reactor.hh"
#include "core/app-template.hh"
#include "core/future-util.hh"
#include "core/distributed.hh"
#include "core/semaphore.hh"
#include "core/future-util.hh"
#include <chrono>

template <typename... Args>
void http_debug(const char* fmt, Args&&... args) {
#if HTTP_DEBUG
    print(fmt, std::forward<Args>(args)...);
#endif
}

class http_client {
private:
    unsigned _duration;
    unsigned _conn_per_core;
    unsigned _reqs_per_conn;
    std::vector<connected_socket> _sockets;
    semaphore _conn_connected{0};
    semaphore _conn_finished{0};
    timer<> _run_timer;
    bool _timer_based;
    bool _timer_done{false};
    uint64_t _total_reqs{0};
public:
    http_client(unsigned duration, unsigned total_conn, unsigned reqs_per_conn)
        : _duration(duration)
        , _conn_per_core(total_conn / smp::count)
        , _reqs_per_conn(reqs_per_conn)
        , _run_timer([this] { _timer_done = true; })
        , _timer_based(reqs_per_conn == 0) {
    }

    class connection {
    private:
        connected_socket _fd;
        input_stream<char> _read_buf;
        output_stream<char> _write_buf;
        http_response_parser _parser;
        http_client* _http_client;
        uint64_t _nr_done{0};
    public:
        connection(connected_socket&& fd, http_client* client)
            : _fd(std::move(fd))
            , _read_buf(_fd.input())
            , _write_buf(_fd.output())
            , _http_client(client){
        }

        uint64_t nr_done() {
            return _nr_done;
        }

        future<> do_req() {
            return _write_buf.write("GET / HTTP/1.1\r\nHost: 127.0.0.1:10000\r\n\r\n").then([this] {
                return _write_buf.flush();
            }).then([this] {
                _parser.init();
                return _read_buf.consume(_parser).then([this] {
                    // Read HTTP response header first
                    if (_parser.eof()) {
                        return make_ready_future<>();
                    }
                    auto _rsp = _parser.get_parsed_response();
                    auto it = _rsp->_headers.find("Content-Length");
                    if (it == _rsp->_headers.end()) {
                        print("Error: HTTP response does not contain: Content-Length\n");
                        return make_ready_future<>();
                    }
                    auto content_len = std::stoi(it->second);
                    http_debug("Content-Length = %d\n", content_len);
                    // Read HTTP response body
                    return _read_buf.read_exactly(content_len).then([this] (temporary_buffer<char> buf) {
                        _nr_done++;
                        http_debug("%s\n", buf.get());
                        if (_http_client->done(_nr_done)) {
                            return make_ready_future();
                        } else {
                            return do_req();
                        }
                    });
                });
            });
        }
    };

    future<uint64_t> total_reqs() {
        print("Requests on cpu %2d: %ld\n", engine().cpu_id(), _total_reqs);
        return make_ready_future<uint64_t>(_total_reqs);
    }

    bool done(uint64_t nr_done) {
        if (_timer_based) {
            return _timer_done;
        } else {
            return nr_done >= _reqs_per_conn;
        }
    }

    future<> connect(ipv4_addr server_addr) {
        // Establish all the TCP connections first
        for (unsigned i = 0; i < _conn_per_core; i++) {
            engine().net().connect(make_ipv4_address(server_addr)).then([this] (connected_socket fd) {
                _sockets.push_back(std::move(fd));
                http_debug("Established connection %6d on cpu %3d\n", _conn_connected.current(), engine().cpu_id());
                _conn_connected.signal();
            });
        }
        return _conn_connected.wait(_conn_per_core);
    }

    future<> run() {
        // All connected, start HTTP request
        http_debug("Established all %6d tcp connections on cpu %3d\n", _conn_per_core, engine().cpu_id());
        if (_timer_based) {
            _run_timer.arm(std::chrono::seconds(_duration));
        }
        for (auto&& fd : _sockets) {
            auto conn = new connection(std::move(fd), this);
            conn->do_req().rescue([this, conn] (auto get_ex) {
                http_debug("Finished connection %6d on cpu %3d\n", _conn_finished.current(), engine().cpu_id());
                _total_reqs += conn->nr_done();
                _conn_finished.signal();
                delete conn;
                try {
                    get_ex();
                } catch (std::exception& ex) {
                    print("http request error: %s\n", ex.what());
                }
            });
        }

        // All finished
        return _conn_finished.wait(_conn_per_core);
    }
    future<> stop() {
        return make_ready_future();
    }
};

namespace bpo = boost::program_options;

int main(int ac, char** av) {
    app_template app;
    app.add_options()
        ("server,s", bpo::value<std::string>()->default_value("192.168.66.100:10000"), "Server address")
        ("conn,c", bpo::value<unsigned>()->default_value(100), "total connections")
        ("reqs,r", bpo::value<unsigned>()->default_value(0), "reqs per connection")
        ("duration,d", bpo::value<unsigned>()->default_value(10), "duration of the test in seconds)");

    return app.run(ac, av, [&app] {
        auto& config = app.configuration();
        auto server = config["server"].as<std::string>();
        auto reqs_per_conn = config["reqs"].as<unsigned>();
        auto total_conn= config["conn"].as<unsigned>();
        auto duration = config["duration"].as<unsigned>();

        if (total_conn % smp::count != 0) {
            print("Error: conn needs to be n * cpu_nr\n");
            return engine().exit(0);
        }

        auto http_clients = new distributed<http_client>;

        // Start http requests on all the cores
        auto started = std::chrono::high_resolution_clock::now();
        print("========== http_client ============\n");
        print("Server: %s\n", server);
        print("Connections: %u\n", total_conn);
        print("Requests/connection: %s\n", reqs_per_conn == 0 ? "dynamic (timer based)" : std::to_string(reqs_per_conn));
        http_clients->start(std::ref(duration), std::ref(total_conn), std::ref(reqs_per_conn)).then([http_clients, started, server] {
            return http_clients->invoke_on_all(&http_client::connect, ipv4_addr{server});
        }).then([http_clients] {
            return http_clients->invoke_on_all(&http_client::run);
        }).then([http_clients] {
            return http_clients->map_reduce(adder<uint64_t>(), &http_client::total_reqs);
        }).then([http_clients, started] (auto total_reqs) {
           // All the http requests are finished
           auto finished = std::chrono::high_resolution_clock::now();
           auto elapsed = finished - started;
           auto secs = static_cast<double>(elapsed.count() / 1000000000.0);
           print("Total cpus: %u\n", smp::count);
           print("Total requests: %u\n", total_reqs);
           print("Total time: %f\n", secs);
           print("Requests/sec: %f\n", static_cast<double>(total_reqs) / secs);
           print("==========     done     ============\n");
           http_clients->stop().then([http_clients] {
               // FIXME: If we call engine().exit(0) here to exit when
               // requests are done. The tcp connection will not be closed
               // properly, becasue we exit too earily and the FIN packets are
               // not exchanged.
                delete http_clients;
           });
        });
    });
}
<|endoftext|>


<commit_before>
#include "AConfig.h"
#if(HAS_STD_PILOT)
#include "Device.h"
#include "Pin.h"
#include "Pilot.h"
#include "Timer.h"

Timer pilotTimer;
bool _headingHoldEnabled = false;
int  _headingHoldTarget = 0;
int hdg = 0;
int hdg_Error;
int raw_Left, raw_Right;
int left, right;  // motor outputs in microseconds, +/-500
int loop_Gain = 1;
int integral_Divisor = 100;
long hdg_Error_Integral = 0;
int tgt_Hdg = 0;
bool _depthHoldEnabled = false;
int _depthHoldTarget = 0;
int depth = 0;
int depth_Error = 0;
int raw_lift =0;
int lift = 0;
int target_depth;
int raw_yaw, yaw;

void Pilot::device_setup(){
  pilotTimer.reset();
  Serial.println(F("log:pilot setup complete;"));
}

void Pilot::device_loop(Command command){
//intended to respond to fly by wire commands: MaintainHeading(); TurnTo(compassheading); DiveTo(depth);
    if( command.cmp("holdHeading_toggle")){
      if (_headingHoldEnabled) {
        _headingHoldEnabled = false;
        raw_Left = 0;
        raw_Right = 0;
        hdg_Error_Integral = 0;  // Reset error integrator
        tgt_Hdg = -500;  // -500 = system not in hdg hold

        int argsToSend[] = {1,00}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
        Serial.println(F("log:heading_hold_disabled;"));

      } else {
        _headingHoldEnabled = true;
        if(command.args[0]==0){
          _headingHoldTarget = navdata::HDGD;
        } else {
          _headingHoldTarget = command.args[1];
        }
        tgt_Hdg = _headingHoldTarget;
        Serial.print(F("log:heading_hold_enabled on="));
        Serial.print(tgt_Hdg);
        Serial.println(';');
      }
      Serial.print(F("targetHeading:"));
      if (_headingHoldEnabled) {
        Serial.print(tgt_Hdg);
      } else {
        Serial.print(DISAABLED);
      }
      Serial.println(';');
    }

    if( command.cmp("holdDepth_toggle")){
      if (_depthHoldEnabled) {
        _depthHoldEnabled = false;
        raw_lift = 0;
        target_depth = 0;

        int argsToSend[] = {1,0}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);
        Serial.println(F("log:depth_hold_disabled;"));

      } else {
        _depthHoldEnabled = true;
        if(command.args[0]==0){
          _depthHoldTarget = navdata::DEAP*100;  //casting to cm
        } else {
          _depthHoldTarget = command.args[1];
        }
        target_depth = _depthHoldTarget;
        Serial.print(F("log:depth_hold_enabled on="));
        Serial.print(target_depth);
        Serial.println(';');
      }
      Serial.print(F("targetDepth:"));
      if (_depthHoldEnabled) {
        Serial.print(target_depth);
      } else {
        Serial.pitnt(DISABLED);
      }
      Serial.println(';');
    }

    if (pilotTimer.elapsed (50)) {

      // Autopilot Test #3 6 Jan 2014
      // Hold vehicle at arbitrary heading
      // Integer math; proportional control plus basic integrator
      // No hysteresis around 180 degree error

      // Check whether hold mode is on

      if (_depthHoldEnabled)
      {
        depth = navdata::DEAP*100;
        depth_Error = target_depth-depth;  //positive error = positive lift = go deaper.

        raw_lift = depth_Error * loop_Gain;
        lift = constrain(raw_lift, -50, 50);

        Serial.println(F("log:dhold pushing command;"));
        Serial.print(F("dp_er:"));
        Serial.print(depth_Error);
        Serial.println(';');
        int argsToSend[] = {1,lift}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);

      }

      if (_headingHoldEnabled)
      {

        // Code for hold mode here
        hdg = navdata::HDGD;

        // Calculate heading error

        hdg_Error = hdg - tgt_Hdg;

        if (hdg_Error > 180)
        {
        hdg_Error = hdg_Error - 360;
        }

        if (hdg_Error < -179)
        {
        hdg_Error = hdg_Error + 360;
        }

        // Run error accumulator (integrator)
        hdg_Error_Integral = hdg_Error_Integral + hdg_Error;

        // Calculator motor outputs
        raw_yaw = -1 * hdg_Error * loop_Gain;

        // raw_Left = raw_Left - (hdg_Error_Integral / integral_Divisor);
        // raw_Right = raw_Right + (hdg_Error_Integral / integral_Divisor);

        // Constrain and output to motors

        yaw = constrain(raw_yaw, -50, 50);
        Serial.println(F("log:hold pushing command;"));
        Serial.print(F("p_er:"));
        Serial.print(hdg_Error);
        Serial.println(';');

        int argsToSend[] = {1,yaw}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
      }

    }
}
#endif

<commit_msg>Revert "Revert "Fix spelling in Arduino""<commit_after>
#include "AConfig.h"
#if(HAS_STD_PILOT)
#include "Device.h"
#include "Pin.h"
#include "Pilot.h"
#include "Timer.h"

Timer pilotTimer;
bool _headingHoldEnabled = false;
int  _headingHoldTarget = 0;
int hdg = 0;
int hdg_Error;
int raw_Left, raw_Right;
int left, right;  // motor outputs in microseconds, +/-500
int loop_Gain = 1;
int integral_Divisor = 100;
long hdg_Error_Integral = 0;
int tgt_Hdg = 0;
bool _depthHoldEnabled = false;
int _depthHoldTarget = 0;
int depth = 0;
int depth_Error = 0;
int raw_lift =0;
int lift = 0;
int target_depth;
int raw_yaw, yaw;

void Pilot::device_setup(){
  pilotTimer.reset();
  Serial.println(F("log:pilot setup complete;"));
}

void Pilot::device_loop(Command command){
//intended to respond to fly by wire commands: MaintainHeading(); TurnTo(compassheading); DiveTo(depth);
    if( command.cmp("holdHeading_toggle")){
      if (_headingHoldEnabled) {
        _headingHoldEnabled = false;
        raw_Left = 0;
        raw_Right = 0;
        hdg_Error_Integral = 0;  // Reset error integrator
        tgt_Hdg = -500;  // -500 = system not in hdg hold

        int argsToSend[] = {1,00}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
        Serial.println(F("log:heading_hold_disabled;"));

      } else {
        _headingHoldEnabled = true;
        if(command.args[0]==0){
          _headingHoldTarget = navdata::HDGD;
        } else {
          _headingHoldTarget = command.args[1];
        }
        tgt_Hdg = _headingHoldTarget;
        Serial.print(F("log:heading_hold_enabled on="));
        Serial.print(tgt_Hdg);
        Serial.println(';');
      }
      Serial.print(F("targetHeading:"));
      if (_headingHoldEnabled) {
        Serial.print(tgt_Hdg);
      } else {
        Serial.print(DISABLED);
      }
      Serial.println(';');
    }

    if( command.cmp("holdDepth_toggle")){
      if (_depthHoldEnabled) {
        _depthHoldEnabled = false;
        raw_lift = 0;
        target_depth = 0;

        int argsToSend[] = {1,0}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);
        Serial.println(F("log:depth_hold_disabled;"));

      } else {
        _depthHoldEnabled = true;
        if(command.args[0]==0){
          _depthHoldTarget = navdata::DEAP*100;  //casting to cm
        } else {
          _depthHoldTarget = command.args[1];
        }
        target_depth = _depthHoldTarget;
        Serial.print(F("log:depth_hold_enabled on="));
        Serial.print(target_depth);
        Serial.println(';');
      }
      Serial.print(F("targetDepth:"));
      if (_depthHoldEnabled) {
        Serial.print(target_depth);
      } else {
        Serial.pitnt(DISABLED);
      }
      Serial.println(';');
    }

    if (pilotTimer.elapsed (50)) {

      // Autopilot Test #3 6 Jan 2014
      // Hold vehicle at arbitrary heading
      // Integer math; proportional control plus basic integrator
      // No hysteresis around 180 degree error

      // Check whether hold mode is on

      if (_depthHoldEnabled)
      {
        depth = navdata::DEAP*100;
        depth_Error = target_depth-depth;  //positive error = positive lift = go deaper.

        raw_lift = depth_Error * loop_Gain;
        lift = constrain(raw_lift, -50, 50);

        Serial.println(F("log:dhold pushing command;"));
        Serial.print(F("dp_er:"));
        Serial.print(depth_Error);
        Serial.println(';');
        int argsToSend[] = {1,lift}; //include number of parms as last parm
        command.pushCommand("lift",argsToSend);

      }

      if (_headingHoldEnabled)
      {

        // Code for hold mode here
        hdg = navdata::HDGD;

        // Calculate heading error

        hdg_Error = hdg - tgt_Hdg;

        if (hdg_Error > 180)
        {
        hdg_Error = hdg_Error - 360;
        }

        if (hdg_Error < -179)
        {
        hdg_Error = hdg_Error + 360;
        }

        // Run error accumulator (integrator)
        hdg_Error_Integral = hdg_Error_Integral + hdg_Error;

        // Calculator motor outputs
        raw_yaw = -1 * hdg_Error * loop_Gain;

        // raw_Left = raw_Left - (hdg_Error_Integral / integral_Divisor);
        // raw_Right = raw_Right + (hdg_Error_Integral / integral_Divisor);

        // Constrain and output to motors

        yaw = constrain(raw_yaw, -50, 50);
        Serial.println(F("log:hold pushing command;"));
        Serial.print(F("p_er:"));
        Serial.print(hdg_Error);
        Serial.println(';');

        int argsToSend[] = {1,yaw}; //include number of parms as last parm
        command.pushCommand("yaw",argsToSend);
      }

    }
}
#endif

<|endoftext|>


<commit_before><commit_msg>Let the debugger work again. Error was due to me not noticing the change in debugger URL.<commit_after><|endoftext|>


<commit_before>// Copyright 2011 The Avalon Project Authors. All rights reserved.
// Use of this source code is governed by the Apache License 2.0
// that can be found in the LICENSE file.
//
// Main loop for helmsman: open client sockets to wind_sensor and imu, server
// socket for helsman clients (e.g. the skipper and sysmon) and shovel
// data between all open file descriptors and the main controller.
//

#include <errno.h>
#include <math.h>
#include <signal.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <syslog.h>
#include <time.h>
#include <unistd.h>

#include "io2/lib/linebuffer.h"

#include "proto/gps.h"
#include "proto/rudder.h"
#include "proto/wind.h"
#include "proto/imu.h"
#include "proto/helmsman.h"
#include "proto/helmsman_status.h"
#include "proto/remote.h"
#include "skipper_input.h"

#include "common/convert.h"
#include "common/now.h"
#include "common/unknown.h"
#include "sampling_period.h"
#include "ship_control.h"

extern int debug;

// -----------------------------------------------------------------------------
namespace {

// -----------------------------------------------------------------------------
//   Together with getopt in main, this is our minimalistic UI
// -----------------------------------------------------------------------------

//const char* version = "$Id: $";
const char* argv0;
int verbose = 0;

void crash(const char* fmt, ...) {
  va_list ap;
  char buf[1000];
  va_start(ap, fmt);
  vsnprintf(buf, sizeof(buf), fmt, ap);
  syslog(LOG_CRIT, "%s%s%s\n", buf,
	 (errno) ? ": " : "",
	 (errno) ? strerror(errno):"" );
  exit(1);
  va_end(ap);
  return;
}

static void bus_fault(int i) { crash("bus fault"); }
static void segv_fault(int i) { crash("segv fault"); }

void usage(void) {
  fprintf(stderr,
    "usage: [plug /path/to/bus] %s [options]\n"
    "options:\n"
    "\t-d debug\n"
    , argv0);
  exit(2);
}

static const int64_t kPeriodMicros = kSamplingPeriod * 1E6;

void AdvanceCallTime(int64_t* next_call_micros) {
  *next_call_micros += kPeriodMicros;
  if (now_micros() > *next_call_micros) {
    syslog(LOG_WARNING, "Irkss!! Too late by %lld micros\n", (now_micros() - *next_call_micros));
    *next_call_micros = now_micros();
  }
}

bool CalculateTimeOut(int64_t next_call_micros, struct timespec* timeout) {
  int64_t now = now_micros();
  int64_t until_call = next_call_micros > now ? next_call_micros - now : 0;
  if (until_call > kPeriodMicros)
    until_call = kPeriodMicros;
  CHECK(kSamplingPeriod < 1.0);
  timeout->tv_sec = 0;
  timeout->tv_nsec = 1000 * (until_call % 1000000LL);
  return until_call > 0;
}

void HandleRemoteControl(RemoteProto remote, int* control_mode) {
  if (remote.command != *control_mode)
    syslog(LOG_NOTICE, "Helmsman switched to control mode %d\n", remote.command);
  switch (remote.command) {
    case kNormalControlMode:
    case kOverrideSkipperMode:
      *control_mode = remote.command;
      ShipControl::Normal();
      break;
    case kDockingControlMode:
      *control_mode = remote.command;
       ShipControl::Docking();
      break;
    case kBrakeControlMode:
    case kPowerCycleMode:
      *control_mode = remote.command;
      ShipControl::Brake();
      break;
    case kIdleHelmsmanMode:
      *control_mode = remote.command;
      ShipControl::Idle();
      break;
  default:
    syslog(LOG_WARNING, "Illegal remote control: %d", remote.command);
  }
}

void HandleRemoteControlFailSafe(int64_t last_remote_message_millis, int* control_mode) {
  // See the alive_timer_ in remote_control/mainwindow.cc .
  const int64_t kRemoteControlTimeOutSeconds = 5;  // We get a message every 2 s and we are allowed to miss one.
  if ((kIdleHelmsmanMode == *control_mode ||
       kOverrideSkipperMode == *control_mode) &&
      now_ms() > last_remote_message_millis + kRemoteControlTimeOutSeconds * 1000) {
    *control_mode = kBrakeControlMode;
    syslog(LOG_WARNING, "helsman main: remote control communication timeout, braking");
    ShipControl::Brake();
  }
}

} // namespace

// -----------------------------------------------------------------------------
//   Main.
// -----------------------------------------------------------------------------
int main(int argc, char* argv[]) {

  int ch;
  argv0 = strrchr(argv[0], '/');
  if (argv0) ++argv0; else argv0 = argv[0];

  while ((ch = getopt(argc, argv, "dhv")) != -1){
    switch (ch) {
    case 'd': ++debug; break;
    case 'v': ++verbose; break;
    case 'h':
    default:
      usage();
    }
  }

  argv += optind;
  argc -= optind;

  if (argc != 0) usage();

  openlog(argv0, debug?LOG_PERROR:0, LOG_LOCAL0);
  if(!debug) setlogmask(LOG_UPTO(LOG_NOTICE));

  if(setvbuf(stdout, NULL, _IOLBF, 0))
    syslog(LOG_WARNING, "Failed to make stdout line-buffered.");

  if (signal(SIGBUS, bus_fault) == SIG_ERR)  crash("signal(SIGBUS)");
  if (signal(SIGSEGV, segv_fault) == SIG_ERR)  crash("signal(SIGSEGV)");
  if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) crash("signal");

  syslog(LOG_NOTICE, "Helmsman started");

  ControllerInput ctrl_in;
  ControllerOutput ctrl_out;  // in this scope because it keeps the statistics.

  struct WindProto wind_sensor  = INIT_WINDPROTO;
  struct CompassProto compass  = INIT_COMPASSPROTO;
  struct RudderProto sts = INIT_RUDDERPROTO;
  struct IMUProto imu    = INIT_IMUPROTO;
  struct HelmsmanCtlProto ctl = INIT_HELMSMANCTLPROTO;
  struct RemoteProto remote = INIT_REMOTEPROTO;
  struct GPSProto gps = INIT_GPSPROTO;
  ctrl_in.alpha_star_rad = Deg2Rad(225);  // Going SouthWest is a good guess (and breaks up a deadlock)
  int control_mode = kNormalControlMode;
  int64_t last_remote_message_millis = now_ms();

  int loops = 0;

  // Run ship controller exactly once every 100ms
  int64_t next_call_micros = now_micros() + kPeriodMicros;
  struct timespec timeout;
  CalculateTimeOut(next_call_micros, &timeout);

  struct LineBuffer lbuf;
  memset(&lbuf, 0, sizeof lbuf);

  for (;;) {

    fd_set rfds;
    FD_ZERO(&rfds);
    FD_SET(fileno(stdin), &rfds);
    sigset_t empty_mask;
    sigemptyset(&empty_mask);
    int r = pselect(fileno(stdin) + 1, &rfds,  NULL, NULL, &timeout, &empty_mask);
    if (r == -1 && errno != EINTR) crash("pselect");

    if (debug>2) syslog(LOG_DEBUG, "Woke up %d\n", r);

    if (r == 1) {
      r = lb_readfd(&lbuf, fileno(stdin));
      if (r == EOF) break;
      if (r == EAGAIN) continue;
      if (r != 0) crash("reading stdin");
    }

    char line[1024];
    while(lb_getline(line, sizeof line, &lbuf) > 0) {
      int nn = 0;
      if (sscanf(line, IFMT_WINDPROTO(&wind_sensor, &nn)) > 0) {
	ctrl_in.wind_sensor.Reset();
	ctrl_in.wind_sensor.alpha_deg = SymmetricDeg(NormalizeDeg(wind_sensor.angle_deg));
	ctrl_in.wind_sensor.mag_m_s = wind_sensor.speed_m_s;
	ctrl_in.wind_sensor.valid = wind_sensor.valid;
      } else if (sscanf(line, IFMT_IMUPROTO(&imu, &nn)) > 0) {
	ctrl_in.imu.Reset();
	ctrl_in.imu.FromProto(imu);
      } else if (sscanf(line, IFMT_RUDDERPROTO_STS(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_rudder_left_rad  = Deg2Rad(sts.rudder_l_deg);
	ctrl_in.drives.gamma_rudder_right_rad = Deg2Rad(sts.rudder_r_deg);
	ctrl_in.drives.gamma_sail_rad         = Deg2Rad(sts.sail_deg);
	ctrl_in.drives.homed_rudder_left = !isnan(sts.rudder_l_deg);
	ctrl_in.drives.homed_rudder_right = !isnan(sts.rudder_r_deg);
	ctrl_in.drives.homed_sail = !isnan(sts.sail_deg);
      } else if (sscanf(line, IFMT_STATUS_LEFT(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_rudder_left_rad  = Deg2Rad(sts.rudder_l_deg);
	ctrl_in.drives.homed_rudder_left = !isnan(sts.rudder_l_deg);
      } else if (sscanf(line, IFMT_STATUS_RIGHT(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_rudder_right_rad  = Deg2Rad(sts.rudder_r_deg);
	ctrl_in.drives.homed_rudder_right = !isnan(sts.rudder_r_deg);
      } else if (sscanf(line, IFMT_STATUS_SAIL(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_sail_rad  = Deg2Rad(sts.sail_deg);
	ctrl_in.drives.homed_sail = !isnan(sts.sail_deg);
      } else if (sscanf(line, IFMT_COMPASSPROTO(&compass, &nn)) > 0) {
	ctrl_in.compass_sensor.phi_z_rad  = Deg2Rad(compass.yaw_deg);
      } else if (sscanf(line, IFMT_GPSPROTO(&gps, &nn)) > 0) {
        ctrl_in.gps.latitude_deg = gps.lat_deg;
        ctrl_in.gps.longitude_deg = gps.lng_deg;
        ctrl_in.gps.speed_m_s = gps.speed_m_s;
	    ctrl_in.gps.cog_rad = Deg2Rad(gps.cog_deg);
      } else if (sscanf(line, IFMT_HELMSMANCTLPROTO(&ctl, &nn)) > 0) {
	    if (control_mode != kOverrideSkipperMode &&
	        !isnan(ctl.alpha_star_deg))
	      ctrl_in.alpha_star_rad = Deg2Rad(ctl.alpha_star_deg);
      } else if (sscanf(line, IFMT_REMOTEPROTO(&remote, &nn)) > 0) {
    HandleRemoteControl(remote, &control_mode);
    last_remote_message_millis = now_ms();
	if (control_mode == kOverrideSkipperMode &&
	    !isnan(remote.alpha_star_deg))
	  ctrl_in.alpha_star_rad = Deg2Rad(remote.alpha_star_deg);
      } else {
  // For debugging only:
	// Any unexpected input (messages not sent to us, or debug output that
	// accidentally was sent to stdout instead of stderr comes here.
	// syslog(LOG_DEBUG, "Unreadable input \n>>>%s<<<\n", line);
      }
    }

    HandleRemoteControlFailSafe(last_remote_message_millis, &control_mode);

    if (!CalculateTimeOut(next_call_micros, &timeout)) {
      ctrl_out.Reset();
      ShipControl::Run(ctrl_in, &ctrl_out);
      AdvanceCallTime(&next_call_micros);

      if (!ShipControl::Idling()) {
        RudderProto ctl;
        ctrl_out.drives_reference.ToProto(&ctl);
        ctl.timestamp_ms = now_ms();
        printf(OFMT_RUDDERPROTO_CTL(ctl));
      }
      // One minute should be enough to execute the last direction change.
      if (loops % static_cast<int>(60.0 / kSamplingPeriod) == 0) {
        SkipperInput to_skipper(
            now_ms(),
            ctrl_out.skipper_input.latitude_deg,
            ctrl_out.skipper_input.longitude_deg,
            ctrl_out.skipper_input.angle_true_deg,
            ctrl_out.skipper_input.mag_true_kn
        );
        fprintf(stderr, "Checking lat lon %lf %lf \n",ctrl_out.skipper_input.latitude_deg,  ctrl_out.skipper_input.longitude_deg);
        if (to_skipper.Valid()) {
          printf("%s", to_skipper.ToString().c_str());
        }
      }
      if (loops % 20 == 5) {
        HelmsmanStatusProto hsts = INIT_HELMSMAN_STATUSPROTO;
        ctrl_out.status.ToProto(&hsts);
        hsts.timestamp_ms = now_ms();
        printf(OFMT_HELMSMAN_STATUSPROTO(hsts));
      }

      ++loops;
      loops %= 1000;
    }
  }  // for ever

  crash("Main loop exit");

  return 0;
}
<commit_msg>helmsman: fix bug in reportingperiod<commit_after>// Copyright 2011 The Avalon Project Authors. All rights reserved.
// Use of this source code is governed by the Apache License 2.0
// that can be found in the LICENSE file.
//
// Main loop for helmsman: open client sockets to wind_sensor and imu, server
// socket for helsman clients (e.g. the skipper and sysmon) and shovel
// data between all open file descriptors and the main controller.
//

#include <errno.h>
#include <math.h>
#include <signal.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <syslog.h>
#include <time.h>
#include <unistd.h>

#include "io2/lib/linebuffer.h"

#include "proto/gps.h"
#include "proto/rudder.h"
#include "proto/wind.h"
#include "proto/imu.h"
#include "proto/helmsman.h"
#include "proto/helmsman_status.h"
#include "proto/remote.h"
#include "skipper_input.h"

#include "common/convert.h"
#include "common/now.h"
#include "common/unknown.h"
#include "sampling_period.h"
#include "ship_control.h"

extern int debug;

// -----------------------------------------------------------------------------
namespace {

// -----------------------------------------------------------------------------
//   Together with getopt in main, this is our minimalistic UI
// -----------------------------------------------------------------------------

//const char* version = "$Id: $";
const char* argv0;
int verbose = 0;

void crash(const char* fmt, ...) {
  va_list ap;
  char buf[1000];
  va_start(ap, fmt);
  vsnprintf(buf, sizeof(buf), fmt, ap);
  syslog(LOG_CRIT, "%s%s%s\n", buf,
	 (errno) ? ": " : "",
	 (errno) ? strerror(errno):"" );
  exit(1);
  va_end(ap);
  return;
}

static void bus_fault(int i) { crash("bus fault"); }
static void segv_fault(int i) { crash("segv fault"); }

void usage(void) {
  fprintf(stderr,
    "usage: [plug /path/to/bus] %s [options]\n"
    "options:\n"
    "\t-d debug\n"
    , argv0);
  exit(2);
}

static const int64_t kPeriodMicros = kSamplingPeriod * 1E6;

void AdvanceCallTime(int64_t* next_call_micros) {
  *next_call_micros += kPeriodMicros;
  if (now_micros() > *next_call_micros) {
    syslog(LOG_WARNING, "Irkss!! Too late by %lld micros\n", (now_micros() - *next_call_micros));
    *next_call_micros = now_micros();
  }
}

bool CalculateTimeOut(int64_t next_call_micros, struct timespec* timeout) {
  int64_t now = now_micros();
  int64_t until_call = next_call_micros > now ? next_call_micros - now : 0;
  if (until_call > kPeriodMicros)
    until_call = kPeriodMicros;
  CHECK(kSamplingPeriod < 1.0);
  timeout->tv_sec = 0;
  timeout->tv_nsec = 1000 * (until_call % 1000000LL);
  return until_call > 0;
}

void HandleRemoteControl(RemoteProto remote, int* control_mode) {
  if (remote.command != *control_mode)
    syslog(LOG_NOTICE, "Helmsman switched to control mode %d\n", remote.command);
  switch (remote.command) {
    case kNormalControlMode:
    case kOverrideSkipperMode:
      *control_mode = remote.command;
      ShipControl::Normal();
      break;
    case kDockingControlMode:
      *control_mode = remote.command;
       ShipControl::Docking();
      break;
    case kBrakeControlMode:
    case kPowerCycleMode:
      *control_mode = remote.command;
      ShipControl::Brake();
      break;
    case kIdleHelmsmanMode:
      *control_mode = remote.command;
      ShipControl::Idle();
      break;
  default:
    syslog(LOG_WARNING, "Illegal remote control: %d", remote.command);
  }
}

void HandleRemoteControlFailSafe(int64_t last_remote_message_millis, int* control_mode) {
  // See the alive_timer_ in remote_control/mainwindow.cc .
  const int64_t kRemoteControlTimeOutSeconds = 5;  // We get a message every 2 s and we are allowed to miss one.
  if ((kIdleHelmsmanMode == *control_mode ||
       kOverrideSkipperMode == *control_mode) &&
      now_ms() > last_remote_message_millis + kRemoteControlTimeOutSeconds * 1000) {
    *control_mode = kBrakeControlMode;
    syslog(LOG_WARNING, "helsman main: remote control communication timeout, braking");
    ShipControl::Brake();
  }
}

} // namespace

// -----------------------------------------------------------------------------
//   Main.
// -----------------------------------------------------------------------------
int main(int argc, char* argv[]) {

  int ch;
  argv0 = strrchr(argv[0], '/');
  if (argv0) ++argv0; else argv0 = argv[0];

  while ((ch = getopt(argc, argv, "dhv")) != -1){
    switch (ch) {
    case 'd': ++debug; break;
    case 'v': ++verbose; break;
    case 'h':
    default:
      usage();
    }
  }

  argv += optind;
  argc -= optind;

  if (argc != 0) usage();

  openlog(argv0, debug?LOG_PERROR:0, LOG_LOCAL0);
  if(!debug) setlogmask(LOG_UPTO(LOG_NOTICE));

  if(setvbuf(stdout, NULL, _IOLBF, 0))
    syslog(LOG_WARNING, "Failed to make stdout line-buffered.");

  if (signal(SIGBUS, bus_fault) == SIG_ERR)  crash("signal(SIGBUS)");
  if (signal(SIGSEGV, segv_fault) == SIG_ERR)  crash("signal(SIGSEGV)");
  if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) crash("signal");

  syslog(LOG_NOTICE, "Helmsman started");

  ControllerInput ctrl_in;
  ControllerOutput ctrl_out;  // in this scope because it keeps the statistics.

  struct WindProto wind_sensor  = INIT_WINDPROTO;
  struct CompassProto compass  = INIT_COMPASSPROTO;
  struct RudderProto sts = INIT_RUDDERPROTO;
  struct IMUProto imu    = INIT_IMUPROTO;
  struct HelmsmanCtlProto ctl = INIT_HELMSMANCTLPROTO;
  struct RemoteProto remote = INIT_REMOTEPROTO;
  struct GPSProto gps = INIT_GPSPROTO;
  ctrl_in.alpha_star_rad = Deg2Rad(225);  // Going SouthWest is a good guess (and breaks up a deadlock)
  int control_mode = kNormalControlMode;
  int64_t last_remote_message_millis = now_ms();

  int loops = 0;

  // Run ship controller exactly once every 100ms
  int64_t next_call_micros = now_micros() + kPeriodMicros;
  struct timespec timeout;
  CalculateTimeOut(next_call_micros, &timeout);

  struct LineBuffer lbuf;
  memset(&lbuf, 0, sizeof lbuf);

  for (;;) {

    fd_set rfds;
    FD_ZERO(&rfds);
    FD_SET(fileno(stdin), &rfds);
    sigset_t empty_mask;
    sigemptyset(&empty_mask);
    int r = pselect(fileno(stdin) + 1, &rfds,  NULL, NULL, &timeout, &empty_mask);
    if (r == -1 && errno != EINTR) crash("pselect");

    if (debug>2) syslog(LOG_DEBUG, "Woke up %d\n", r);

    if (r == 1) {
      r = lb_readfd(&lbuf, fileno(stdin));
      if (r == EOF) break;
      if (r == EAGAIN) continue;
      if (r != 0) crash("reading stdin");
    }

    char line[1024];
    while(lb_getline(line, sizeof line, &lbuf) > 0) {
      int nn = 0;
      if (sscanf(line, IFMT_WINDPROTO(&wind_sensor, &nn)) > 0) {
	ctrl_in.wind_sensor.Reset();
	ctrl_in.wind_sensor.alpha_deg = SymmetricDeg(NormalizeDeg(wind_sensor.angle_deg));
	ctrl_in.wind_sensor.mag_m_s = wind_sensor.speed_m_s;
	ctrl_in.wind_sensor.valid = wind_sensor.valid;
      } else if (sscanf(line, IFMT_IMUPROTO(&imu, &nn)) > 0) {
	ctrl_in.imu.Reset();
	ctrl_in.imu.FromProto(imu);
      } else if (sscanf(line, IFMT_RUDDERPROTO_STS(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_rudder_left_rad  = Deg2Rad(sts.rudder_l_deg);
	ctrl_in.drives.gamma_rudder_right_rad = Deg2Rad(sts.rudder_r_deg);
	ctrl_in.drives.gamma_sail_rad         = Deg2Rad(sts.sail_deg);
	ctrl_in.drives.homed_rudder_left = !isnan(sts.rudder_l_deg);
	ctrl_in.drives.homed_rudder_right = !isnan(sts.rudder_r_deg);
	ctrl_in.drives.homed_sail = !isnan(sts.sail_deg);
      } else if (sscanf(line, IFMT_STATUS_LEFT(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_rudder_left_rad  = Deg2Rad(sts.rudder_l_deg);
	ctrl_in.drives.homed_rudder_left = !isnan(sts.rudder_l_deg);
      } else if (sscanf(line, IFMT_STATUS_RIGHT(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_rudder_right_rad  = Deg2Rad(sts.rudder_r_deg);
	ctrl_in.drives.homed_rudder_right = !isnan(sts.rudder_r_deg);
      } else if (sscanf(line, IFMT_STATUS_SAIL(&sts, &nn)) > 0) {
	ctrl_in.drives.gamma_sail_rad  = Deg2Rad(sts.sail_deg);
	ctrl_in.drives.homed_sail = !isnan(sts.sail_deg);
      } else if (sscanf(line, IFMT_COMPASSPROTO(&compass, &nn)) > 0) {
	ctrl_in.compass_sensor.phi_z_rad  = Deg2Rad(compass.yaw_deg);
      } else if (sscanf(line, IFMT_GPSPROTO(&gps, &nn)) > 0) {
        ctrl_in.gps.latitude_deg = gps.lat_deg;
        ctrl_in.gps.longitude_deg = gps.lng_deg;
        ctrl_in.gps.speed_m_s = gps.speed_m_s;
	    ctrl_in.gps.cog_rad = Deg2Rad(gps.cog_deg);
      } else if (sscanf(line, IFMT_HELMSMANCTLPROTO(&ctl, &nn)) > 0) {
	    if (control_mode != kOverrideSkipperMode &&
	        !isnan(ctl.alpha_star_deg))
	      ctrl_in.alpha_star_rad = Deg2Rad(ctl.alpha_star_deg);
      } else if (sscanf(line, IFMT_REMOTEPROTO(&remote, &nn)) > 0) {
    HandleRemoteControl(remote, &control_mode);
    last_remote_message_millis = now_ms();
	if (control_mode == kOverrideSkipperMode &&
	    !isnan(remote.alpha_star_deg))
	  ctrl_in.alpha_star_rad = Deg2Rad(remote.alpha_star_deg);
      } else {
  // For debugging only:
	// Any unexpected input (messages not sent to us, or debug output that
	// accidentally was sent to stdout instead of stderr comes here.
	// syslog(LOG_DEBUG, "Unreadable input \n>>>%s<<<\n", line);
      }
    }

    HandleRemoteControlFailSafe(last_remote_message_millis, &control_mode);

    if (!CalculateTimeOut(next_call_micros, &timeout)) {
      ctrl_out.Reset();
      ShipControl::Run(ctrl_in, &ctrl_out);
      AdvanceCallTime(&next_call_micros);

      if (!ShipControl::Idling()) {
        RudderProto ctl;
        ctrl_out.drives_reference.ToProto(&ctl);
        ctl.timestamp_ms = now_ms();
        printf(OFMT_RUDDERPROTO_CTL(ctl));
      }
      // One minute should be enough to execute the last direction change.
      if (loops % static_cast<int>(60.0 / kSamplingPeriod) == 0) {
        SkipperInput to_skipper(
            now_ms(),
            ctrl_out.skipper_input.latitude_deg,
            ctrl_out.skipper_input.longitude_deg,
            ctrl_out.skipper_input.angle_true_deg,
            ctrl_out.skipper_input.mag_true_kn
        );
        fprintf(stderr, "Checking lat lon %lf %lf \n",ctrl_out.skipper_input.latitude_deg,  ctrl_out.skipper_input.longitude_deg);
        if (to_skipper.Valid()) {
          printf("%s", to_skipper.ToString().c_str());
        }
      }
      if (loops % 20 == 5) {
        HelmsmanStatusProto hsts = INIT_HELMSMAN_STATUSPROTO;
        ctrl_out.status.ToProto(&hsts);
        hsts.timestamp_ms = now_ms();
        printf(OFMT_HELMSMAN_STATUSPROTO(hsts));
      }

      ++loops;
//      loops %= 1000;
    }
  }  // for ever

  crash("Main loop exit");

  return 0;
}
<|endoftext|>


<commit_before><commit_msg>INTEGRATION: CWS dba24d (1.16.68); FILE MERGED 2007/11/21 12:37:25 oj 1.16.68.2: #i68854# impl TypeSettingInfo for Oracle 2007/11/08 14:12:15 fs 1.16.68.1: during #i81658#: string cleanup<commit_after><|endoftext|>


<commit_before><commit_msg>Removed a blank line to get better commit stats on Ohloh.<commit_after><|endoftext|>