<commit_before>/*
 * =====================================================================================
 *
 *       Filename:  main.cc
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  03/20/2008 12:34:14 AM EDT
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Nikolaos Vasiloglou (NV), nvasil@ieee.org
 *        Company:  Georgia Tech Fastlab-ESP Lab
 *
 * =====================================================================================
 */

#include <string>
#include "fastlib/fastlib.h"
#include "mvu_objectives.h"
#include "../l_bfgs/l_bfgs.h"

int main(int argc, char *argv[]){
  fx_module *fx_root=fx_init(argc, argv, NULL);
  std::string optimized_function=fx_param_str(NULL, "opts/optfun", "mvu");
  std::string data_file=fx_param_str_req(NULL, "opts/data_file");
  Matrix data_mat;
  if (data::Load(data_file.c_str(), &data_mat)==SUCCESS_FAIL) {
    FATAL("Didn't manage to load %s", data_file.c_str());
  }
  NOTIFY("Removing the mean., centering data...");
  OptUtils::RemoveMean(&data_mat);

  fx_module *optfun_node;
  fx_module *l_bfgs_node;
  l_bfgs_node=fx_submodule(fx_root, "opts/l_bfgs");
  optfun_node=fx_submodule(fx_root, "opts/optfun");

  bool pca_preprocess=fx_param_bool(fx_root, "opts/pca_pre", false);
  index_t pca_dimension=fx_param_int(fx_root, "opts/pca_dim", 5);
  bool pca_init=fx_param_bool(fx_root, "opts/pca_init", false);
  Matrix *initial_data;
  if (pca_preprocess==true) {
    NOTIFY("Preprocessing with pca");
    Matrix temp;
    OptUtils::SVDTransform(data_mat, &temp, pca_dimension);
    data_mat.Destruct();
    data_mat.Own(&temp);
  }
  if (pca_init==true) {
    NOTIFY("Preprocessing with pca");
    initial_data = new Matrix();
    index_t new_dimension=fx_param_int(l_bfgs_node, "new_dimension", 2);
    OptUtils::SVDTransform(data_mat, initial_data, new_dimension);
 }

  //we need to insert the number of points
  fx_set_param_int(l_bfgs_node, "num_of_points", data_mat.n_cols());
  std::string result_file=fx_param_str(NULL, "opts/result_file", "result.csv");
  bool done=false;

  if (optimized_function == "mvu") {
    MaxVariance opt_function;
    opt_function.Init(optfun_node, data_mat);
    LBfgs<MaxVariance> engine;
    engine.Init(&opt_function, l_bfgs_node);
    if (pca_init==true) {
      engine.set_coordinates(*initial_data);
    }
    engine.ComputeLocalOptimumBFGS();
    Matrix result;
    engine.GetResults(&result);
    if (data::Save(result_file.c_str(), result)==SUCCESS_FAIL) {
      FATAL("Didn't manage to save %s", result_file.c_str());
    }
    engine.Destruct();
    done=true;
  }
  if (optimized_function=="mvuineq") {
    MaxVarianceInequalityOnFurthest opt_function;
    opt_function.Init(optfun_node, data_mat);
    LBfgs<MaxVarianceInequalityOnFurthest> engine;
    engine.Init(&opt_function, l_bfgs_node);
    if (pca_init==true) {
      engine.set_coordinates(*initial_data);
    }
    engine.ComputeLocalOptimumBFGS();
    Matrix result;
    engine.GetResults(&result);
    if (data::Save(result_file.c_str(), result)==SUCCESS_FAIL) {
      FATAL("Didn't manage to save %s", result_file.c_str());
    }
    engine.Destruct();
    done=true;
  }
  if (optimized_function == "mvfu"){
    MaxFurthestNeighbors opt_function;
    opt_function.Init(optfun_node, data_mat);
    //opt_function.set_lagrange_mult(0.0);
    LBfgs<MaxFurthestNeighbors> engine;
    engine.Init(&opt_function, l_bfgs_node);
    if (pca_init==true) {
      la::Scale(1e-1, initial_data);
      engine.set_coordinates(*initial_data);
    }
    engine.ComputeLocalOptimumBFGS();
    Matrix result;
    engine.GetResults(&result);
    if (data::Save(result_file.c_str(), result)==SUCCESS_FAIL) {
      FATAL("Didn't manage to save %s", result_file.c_str());
    }
    engine.Destruct();
    done=true;
  }
  if (done==false) {
    FATAL("The method you provided %s is not supported",
        optimized_function.c_str());
  }
  if (pca_init==true) {
    delete initial_data;
  }
  fx_done(fx_root);
}
<commit_msg>Fixed a minor warning<commit_after>/*
 * =====================================================================================
 *
 *       Filename:  main.cc
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  03/20/2008 12:34:14 AM EDT
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Nikolaos Vasiloglou (NV), nvasil@ieee.org
 *        Company:  Georgia Tech Fastlab-ESP Lab
 *
 * =====================================================================================
 */

#include <string>
#include "fastlib/fastlib.h"
#include "mvu_objectives.h"
#include "../l_bfgs/l_bfgs.h"

int main(int argc, char *argv[]){
  fx_module *fx_root=fx_init(argc, argv, NULL);
  std::string optimized_function=fx_param_str(NULL, "opts/optfun", "mvu");
  std::string data_file=fx_param_str_req(NULL, "opts/data_file");
  Matrix data_mat;
  if (data::Load(data_file.c_str(), &data_mat)==SUCCESS_FAIL) {
    FATAL("Didn't manage to load %s", data_file.c_str());
  }
  NOTIFY("Removing the mean., centering data...");
  OptUtils::RemoveMean(&data_mat);

  fx_module *optfun_node;
  fx_module *l_bfgs_node;
  l_bfgs_node=fx_submodule(fx_root, "opts/l_bfgs");
  optfun_node=fx_submodule(fx_root, "opts/optfun");

  bool pca_preprocess=fx_param_bool(fx_root, "opts/pca_pre", false);
  index_t pca_dimension=fx_param_int(fx_root, "opts/pca_dim", 5);
  bool pca_init=fx_param_bool(fx_root, "opts/pca_init", false);
  Matrix *initial_data=NULL;
  if (pca_preprocess==true) {
    NOTIFY("Preprocessing with pca");
    Matrix temp;
    OptUtils::SVDTransform(data_mat, &temp, pca_dimension);
    data_mat.Destruct();
    data_mat.Own(&temp);
  }
  if (pca_init==true) {
    NOTIFY("Preprocessing with pca");
    initial_data = new Matrix();
    index_t new_dimension=fx_param_int(l_bfgs_node, "new_dimension", 2);
    OptUtils::SVDTransform(data_mat, initial_data, new_dimension);
 }

  //we need to insert the number of points
  fx_set_param_int(l_bfgs_node, "num_of_points", data_mat.n_cols());
  std::string result_file=fx_param_str(NULL, "opts/result_file", "result.csv");
  bool done=false;

  if (optimized_function == "mvu") {
    MaxVariance opt_function;
    opt_function.Init(optfun_node, data_mat);
    LBfgs<MaxVariance> engine;
    engine.Init(&opt_function, l_bfgs_node);
    if (pca_init==true) {
      engine.set_coordinates(*initial_data);
    }
    engine.ComputeLocalOptimumBFGS();
    Matrix result;
    engine.GetResults(&result);
    if (data::Save(result_file.c_str(), result)==SUCCESS_FAIL) {
      FATAL("Didn't manage to save %s", result_file.c_str());
    }
    engine.Destruct();
    done=true;
  }
  if (optimized_function=="mvuineq") {
    MaxVarianceInequalityOnFurthest opt_function;
    opt_function.Init(optfun_node, data_mat);
    LBfgs<MaxVarianceInequalityOnFurthest> engine;
    engine.Init(&opt_function, l_bfgs_node);
    if (pca_init==true) {
      engine.set_coordinates(*initial_data);
    }
    engine.ComputeLocalOptimumBFGS();
    Matrix result;
    engine.GetResults(&result);
    if (data::Save(result_file.c_str(), result)==SUCCESS_FAIL) {
      FATAL("Didn't manage to save %s", result_file.c_str());
    }
    engine.Destruct();
    done=true;
  }
  if (optimized_function == "mvfu"){
    MaxFurthestNeighbors opt_function;
    opt_function.Init(optfun_node, data_mat);
    //opt_function.set_lagrange_mult(0.0);
    LBfgs<MaxFurthestNeighbors> engine;
    engine.Init(&opt_function, l_bfgs_node);
    if (pca_init==true) {
      la::Scale(1e-1, initial_data);
      engine.set_coordinates(*initial_data);
    }
    engine.ComputeLocalOptimumBFGS();
    Matrix result;
    engine.GetResults(&result);
    if (data::Save(result_file.c_str(), result)==SUCCESS_FAIL) {
      FATAL("Didn't manage to save %s", result_file.c_str());
    }
    engine.Destruct();
    done=true;
  }
  if (done==false) {
    FATAL("The method you provided %s is not supported",
        optimized_function.c_str());
  }
  if (pca_init==true) {
    delete initial_data;
  }
  fx_done(fx_root);
}
<|endoftext|>


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

 Program:   ORFEO Toolbox
 Language:  C++
 Date:      $Date$
 Version:   $Revision$

 Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
 See OTBCopyright.txt for details.

 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.

 =========================================================================*/

#include "otbConcatenateImages.h"

#include <iostream>
#include "otbImage.h"
#include "otbVectorImage.h"
#include "otbImageFileReader.h"
#include "otbObjectList.h"
#include "otbImageList.h"
#include "otbImageListToVectorImageFilter.h"
#include "otbStreamingImageFileWriter.h"
#include "otbStandardFilterWatcher.h"

namespace otb
{

template<typename PixelType>
int generic_main_concatenate(otb::ApplicationOptionsResult* parseResult)
{
  const unsigned int NbImages = parseResult->GetNumberOfParameters("InputImagesList");

  std::ofstream textFile;

  if (parseResult->IsOptionPresent("OutputNameList"))
    {
    std::string textName = parseResult->GetParameterString("OutputNameList");
    textFile.open(textName.c_str());
    }

  std::cout << "Concat of " << NbImages << " images into a multi-band image " << std::endl;

  const unsigned int Dimension = 2;
  typedef otb::Image<PixelType, Dimension>     InputImageType;
  typedef otb::ImageFileReader<InputImageType> ImageReaderType;
  typedef otb::ObjectList<ImageReaderType>     ReaderListType;

  typename ReaderListType::Pointer readerList = ReaderListType::New();

  typedef otb::ImageList<InputImageType> ImageListType;
  typename ImageListType::Pointer imageList = ImageListType::New();

  for (unsigned int i = 0; i < NbImages; i++)
    {
    typename ImageReaderType::Pointer imageReader = ImageReaderType::New();
    imageReader->SetFileName(parseResult->GetParameterString("InputImagesList", i).c_str());

    std::cout << "Adding image " << parseResult->GetParameterString("InputImagesList", i).c_str() << std::endl;
    textFile << parseResult->GetParameterString("InputImagesList", i) << "\n";

    imageReader->UpdateOutputInformation();

    imageList->PushBack(imageReader->GetOutput());
    readerList->PushBack(imageReader);
    }

  textFile.close();

  typedef otb::VectorImage<PixelType, Dimension> VectorImageType;
  typedef otb::ImageListToVectorImageFilter<ImageListType, VectorImageType> ImageListToVectorImageFilterType;
  typename ImageListToVectorImageFilterType::Pointer iL2VI = ImageListToVectorImageFilterType::New();

  iL2VI->SetInput(imageList);

  typedef otb::StreamingImageFileWriter<VectorImageType> ImageWriterType;
  typename ImageWriterType::Pointer imageWriter = ImageWriterType::New();
  imageWriter->SetFileName(parseResult->GetOutputImage().c_str());

  unsigned long size = (10000 * 10000 * sizeof(PixelType)) / NbImages;

  std::cout << "Streaming size: " << size << std::endl;

  imageWriter->SetBufferMemorySize(size);
  imageWriter->SetInput(iL2VI->GetOutput());

  otb::StandardFilterWatcher watcher(imageWriter, "Writing");
  imageWriter->Update();

  return EXIT_SUCCESS;
}

int ConcatenateImages::Describe(ApplicationDescriptor* descriptor)
{
  descriptor->SetName("ConcatenateImages");
  descriptor->SetDescription("Concatenate n images in a multiband image");
  descriptor->AddOptionNParams("InputImagesList", "Images list to concatenate", "il", true,ApplicationDescriptor::InputImage);
  descriptor->AddOutputImage();
  descriptor->AddOption("OutputPixelType",
                        "OutputPixelType: unsigned char (1), short int (2), int (3), float (4),"
                        " double (5), unsigned short int (12), unsigned int (13); default 2",
                        "t", 1, false, ApplicationDescriptor::Integer);
  descriptor->AddOption("OutputNameList",
                        "Text file containing the name of the images used to generate the output in the same order",
                        "ot", 1, false,ApplicationDescriptor::String);

  return EXIT_SUCCESS;
}

int ConcatenateImages::Execute(otb::ApplicationOptionsResult* parseResult)
{
  unsigned int type = 2; //default to short int as the original
                         //program

  if (parseResult->IsOptionPresent("OutputPixelType"))
    {
    type = parseResult->GetParameterUInt("OutputPixelType");
    }

  switch (type)
    {
    case 1:
      generic_main_concatenate<unsigned char> (parseResult);
      break;
    case 2:
      generic_main_concatenate<short int> (parseResult);
      break;
    case 3:
      generic_main_concatenate<int> (parseResult);
      break;
    case 4:
      generic_main_concatenate<float> (parseResult);
      break;
    case 5:
      generic_main_concatenate<double> (parseResult);
      break;
    case 12:
      generic_main_concatenate<unsigned short int> (parseResult);
      break;
    case 13:
      generic_main_concatenate<unsigned int> (parseResult);
      break;
    default:
      generic_main_concatenate<unsigned char> (parseResult);
      break;
    }

  return EXIT_SUCCESS;
}
}
<commit_msg>STYLE<commit_after>/*=========================================================================

 Program:   ORFEO Toolbox
 Language:  C++
 Date:      $Date$
 Version:   $Revision$

 Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
 See OTBCopyright.txt for details.

 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.

 =========================================================================*/

#include "otbConcatenateImages.h"

#include <iostream>
#include "otbImage.h"
#include "otbVectorImage.h"
#include "otbImageFileReader.h"
#include "otbObjectList.h"
#include "otbImageList.h"
#include "otbImageListToVectorImageFilter.h"
#include "otbStreamingImageFileWriter.h"
#include "otbStandardFilterWatcher.h"

namespace otb
{

template<typename PixelType>
int generic_main_concatenate(otb::ApplicationOptionsResult* parseResult)
{
  const unsigned int NbImages = parseResult->GetNumberOfParameters("InputImagesList");

  std::ofstream textFile;

  if (parseResult->IsOptionPresent("OutputNameList"))
    {
    std::string textName = parseResult->GetParameterString("OutputNameList");
    textFile.open(textName.c_str());
    }

  std::cout << "Concat of " << NbImages << " images into a multi-band image " << std::endl;

  const unsigned int Dimension = 2;
  typedef otb::Image<PixelType, Dimension>     InputImageType;
  typedef otb::ImageFileReader<InputImageType> ImageReaderType;
  typedef otb::ObjectList<ImageReaderType>     ReaderListType;

  typename ReaderListType::Pointer readerList = ReaderListType::New();

  typedef otb::ImageList<InputImageType> ImageListType;
  typename ImageListType::Pointer imageList = ImageListType::New();

  for (unsigned int i = 0; i < NbImages; i++)
    {
    typename ImageReaderType::Pointer imageReader = ImageReaderType::New();
    imageReader->SetFileName(parseResult->GetParameterString("InputImagesList", i).c_str());

    std::cout << "Adding image " << parseResult->GetParameterString("InputImagesList", i).c_str() << std::endl;
    textFile << parseResult->GetParameterString("InputImagesList", i) << "\n";

    imageReader->UpdateOutputInformation();

    imageList->PushBack(imageReader->GetOutput());
    readerList->PushBack(imageReader);
    }

  textFile.close();

  typedef otb::VectorImage<PixelType, Dimension> VectorImageType;
  typedef otb::ImageListToVectorImageFilter<ImageListType, VectorImageType> ImageListToVectorImageFilterType;
  typename ImageListToVectorImageFilterType::Pointer iL2VI = ImageListToVectorImageFilterType::New();

  iL2VI->SetInput(imageList);

  typedef otb::StreamingImageFileWriter<VectorImageType> ImageWriterType;
  typename ImageWriterType::Pointer imageWriter = ImageWriterType::New();
  imageWriter->SetFileName(parseResult->GetOutputImage().c_str());

  unsigned long size = (10000 * 10000 * sizeof(PixelType)) / NbImages;

  std::cout << "Streaming size: " << size << std::endl;

  imageWriter->SetBufferMemorySize(size);
  imageWriter->SetInput(iL2VI->GetOutput());

  otb::StandardFilterWatcher watcher(imageWriter, "Writing");
  imageWriter->Update();

  return EXIT_SUCCESS;
}

int ConcatenateImages::Describe(ApplicationDescriptor* descriptor)
{
  descriptor->SetName("ConcatenateImages");
  descriptor->SetDescription("Concatenate n images in a multiband image");
  descriptor->AddOptionNParams("InputImagesList", "Images list to concatenate", "il", true, ApplicationDescriptor::InputImage);
  descriptor->AddOutputImage();
  descriptor->AddOption("OutputPixelType",
                        "OutputPixelType: unsigned char (1), short int (2), int (3), float (4),"
                        " double (5), unsigned short int (12), unsigned int (13); default 2",
                        "t", 1, false, ApplicationDescriptor::Integer);
  descriptor->AddOption("OutputNameList",
                        "Text file containing the name of the images used to generate the output in the same order",
                        "ot", 1, false, ApplicationDescriptor::String);

  return EXIT_SUCCESS;
}

int ConcatenateImages::Execute(otb::ApplicationOptionsResult* parseResult)
{
  unsigned int type = 2; //default to short int as the original
                         //program

  if (parseResult->IsOptionPresent("OutputPixelType"))
    {
    type = parseResult->GetParameterUInt("OutputPixelType");
    }

  switch (type)
    {
    case 1:
      generic_main_concatenate<unsigned char> (parseResult);
      break;
    case 2:
      generic_main_concatenate<short int> (parseResult);
      break;
    case 3:
      generic_main_concatenate<int> (parseResult);
      break;
    case 4:
      generic_main_concatenate<float> (parseResult);
      break;
    case 5:
      generic_main_concatenate<double> (parseResult);
      break;
    case 12:
      generic_main_concatenate<unsigned short int> (parseResult);
      break;
    case 13:
      generic_main_concatenate<unsigned int> (parseResult);
      break;
    default:
      generic_main_concatenate<unsigned char> (parseResult);
      break;
    }

  return EXIT_SUCCESS;
}
}
<|endoftext|>


<commit_before>/*
 * derived from: https://github.com/sabas1080/FXAS21002C_Arduino_Library
 *
 * modified by Francesco Ferraro 01/2016
 * francesco.ferrarogm@gmail.com
 *
*/

#include <Wire.h>
#include <math.h>

#include "FXAS21002C.h"

// Public Methods //////////////////////////////////////////////////////////////

FXAS21002C::FXAS21002C(byte addr)
{
	address = addr;
	gyroODR = GODR_200HZ; //
	gyroFSR = GFS_1000DPS;
}

void FXAS21002C::writeReg(byte reg, byte value)
{
	Wire1.beginTransmission(address);
	Wire1.write(reg);
	Wire1.write(value);
	Wire1.endTransmission();
}

// Reads a register
byte FXAS21002C::readReg(byte reg)
{
	byte value;

	Wire1.beginTransmission(address);
	Wire1.write(reg);
	Wire1.endTransmission();
	Wire1.requestFrom(address, (uint8_t)1);
	value = Wire1.read();
	Wire1.endTransmission();

	return value;
}

void FXAS21002C::readRegs(byte reg, uint8_t count, byte dest[])
{
	uint8_t i = 0;

	Wire1.beginTransmission(address);   // Initialize the Tx buffer
	Wire1.write(reg);            	   // Put slave register address in Tx buffer
	Wire1.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
	Wire1.requestFrom(address, count);  // Read bytes from slave register address

	while (Wire1.available()) {
		dest[i++] = Wire1.read();   // Put read results in the Rx buffer
	}
}

// Read the temperature data
void FXAS21002C::readTempData()
{
	tempData = readReg(FXAS21002C_H_TEMP);
}

// Put the FXAS21002C into standby mode.
// It must be in standby for modifying most registers
void FXAS21002C::standby()
{
	byte c = readReg(FXAS21002C_H_CTRL_REG1);
	writeReg(FXAS21002C_H_CTRL_REG1, c & ~(0x03));// Clear bits 0 and 1; standby mode
}
// Sets the FXAS21000 to active mode.
// Needs to be in this mode to output data
void FXAS21002C::ready()
{
  byte c = readReg(FXAS21002C_H_CTRL_REG1);
  writeReg(FXAS21002C_H_CTRL_REG1, c & ~(0x03));  // Clear bits 0 and 1; standby mode
  writeReg(FXAS21002C_H_CTRL_REG1, c |   0x01);   // Set bit 0 to 1, ready mode; no data acquisition yet
}

// Put the FXAS21002C into active mode.
// Needs to be in this mode to output data.
void FXAS21002C::active()
{
	byte c = readReg(FXAS21002C_H_CTRL_REG1);
	writeReg(FXAS21002C_H_CTRL_REG1, c & ~(0x03));  // Clear bits 0 and 1; standby mode
  	writeReg(FXAS21002C_H_CTRL_REG1, c |   0x02);   // Set bit 1 to 1, active mode; data acquisition enabled
}

void FXAS21002C::init()
{

	/*
	standby();  // Must be in standby to change registers

	// Set up the full scale range to 250, 500, 1000, or 2000 deg/s.
	writeReg(FXAS21002C_H_CTRL_REG0, gyroFSR);
	 // Setup the 3 data rate bits, 4:2
	if (gyroODR < 8)
		writeReg(FXAS21002C_H_CTRL_REG1, gyroODR << 2);

	// Disable FIFO, route FIFO and rate threshold interrupts to INT2, enable data ready interrupt, route to INT1
	// Active HIGH, push-pull output driver on interrupts
	writeReg(FXAS21002C_H_CTRL_REG2, 0x0E);

	 // Set up rate threshold detection; at max rate threshold = FSR; rate threshold = THS*FSR/128
	writeReg(FXAS21002C_H_RT_CFG, 0x07);         // enable rate threshold detection on all axes
	writeReg(FXAS21002C_H_RT_THS, 0x00 | 0x0D);  // unsigned 7-bit THS, set to one-tenth FSR; set clearing debounce counter
	writeReg(FXAS21002C_H_RT_COUNT, 0x04);       // set to 4 (can set up to 255)
	// Configure interrupts 1 and 2
	//writeReg(CTRL_REG3, readReg(CTRL_REG3) & ~(0x02)); // clear bits 0, 1
	//writeReg(CTRL_REG3, readReg(CTRL_REG3) |  (0x02)); // select ACTIVE HIGH, push-pull interrupts
	//writeReg(CTRL_REG4, readReg(CTRL_REG4) & ~(0x1D)); // clear bits 0, 3, and 4
	//writeReg(CTRL_REG4, readReg(CTRL_REG4) |  (0x1D)); // DRDY, Freefall/Motion, P/L and tap ints enabled
	//writeReg(CTRL_REG5, 0x01);  // DRDY on INT1, P/L and taps on INT2
	active();  // Set to active to start reading
	*/

		// write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS21002 in Standby
	 // [7]: ZR_cond=0
	 // [6]: RST=0
	 // [5]: ST=0 self test disabled
	 // [4-2]: DR[2-0]=000 for 800Hz
	 // [1-0]: Active=0, Ready=0 for Standby mode
	 writeReg(FXAS21002_CTRL_REG1, 0x00);

  // write 0000 0000 = 0x00 to CTRL_REG0 to configure range and filters
  // [7-6]: BW[1-0]=00, LPF disabled
  // [5]: SPIW=0 4 wire SPI (irrelevant)
  // [4-3]: SEL[1-0]=00 for 10Hz HPF at 200Hz ODR
  // [2]: HPF_EN=0 disable HPF
  // [1-0]: FS[1-0]=00 for 1600dps (TBD CHANGE TO 2000dps when final trimmed parts available)
	writeReg(FXAS21002_CTRL_REG0, 0b00000001);

  // [7-4]: BW[1-0]=00, LPF disabled
  // [3]: Auto-increment read address pointer roll-over behavior:
  // [2]: External power mode control input
  // [1] disabled
  // [0]: Full-scale range expansion enable
  writeReg(FXAS21002_CTRL_REG3, 0b00000000);

  // write 0000 1010 = 0x0A to CTRL_REG1 to configure 200Hz ODR and enter Active mode
  // [7]: ZR_cond=0
  // [6]: RST=0
  // [5]: ST=0 self test disabled
  // [4-2]: DR[2-0]=010 for 200Hz ODR
  // [1-0]: Active=1, Ready=0 for Active mode
  writeReg(FXAS21002_CTRL_REG1, 0x0A);

}

// Read the gyroscope data
void FXAS21002C::readGyroData()
{
	uint8_t rawData[6];  // x/y/z gyro register data stored here
	readRegs(FXAS21002C_H_OUT_X_MSB, 6, &rawData[0]);  // Read the six raw data registers into data array
	gyroData.x = ((int16_t)( rawData[0] << 8 | rawData[1])) - gBias[0];// >> 2;
	gyroData.y = ((int16_t)( rawData[2] << 8 | rawData[3])) - gBias[1];// >> 2;
	gyroData.z = ((int16_t)( rawData[4] << 8 | rawData[5])) - gBias[2];// >> 2;
}

// Get accelerometer resolution
float FXAS21002C::getGres(void)
{
	switch (gyroFSR)
	{
		// Possible gyro scales (and their register bit settings) are:
  // 250 DPS (11), 500 DPS (10), 1000 DPS (01), and 2000 DPS  (00).
    case GFS_2000DPS:
          gRes = 1600.0/8192.0;
          break;
    case GFS_1000DPS:
          gRes = 0.03125;
          break;
    case GFS_500DPS:
          gRes = 400.0/8192.0;
          break;
    case GFS_250DPS:
          gRes = 200.0/8192.0;
	}
}

void FXAS21002C::calibrate()
{
  int32_t gyro_bias[3] = {0, 0, 0};
  uint16_t ii = 0, fcount = 200;
  int16_t temp[3];

  // Clear all interrupts by reading the data output and STATUS registers
  //readGyroData(temp);
  readReg(FXAS21002C_H_STATUS);

  uint8_t rawData[6];  // x/y/z FIFO accel data stored here
	uint32_t lastRead = micros();
  for(ii = 0; ii < fcount; ii++)   // construct count sums for each axis
  {
	  readRegs(FXAS21002C_H_OUT_X_MSB, 6, &rawData[0]);  // Read the FIFO data registers into data array
	  temp[0] = ((int16_t)( rawData[0] << 8 | rawData[1]));
	  temp[1] = ((int16_t)( rawData[2] << 8 | rawData[3]));
	  temp[2] = ((int16_t)( rawData[4] << 8 | rawData[5]));

	  gyro_bias[0] += (int32_t) temp[0];
	  gyro_bias[1] += (int32_t) temp[1];
	  gyro_bias[2] += (int32_t) temp[2];

	  delayMicroseconds(5000-(micros()-lastRead)); // wait for next data sample at 200 Hz rate
		lastRead = micros();
  }

  gBias[0] = gyro_bias[0] / (int32_t) fcount; // get average values
  gBias[1] = gyro_bias[1] / (int32_t) fcount;
  gBias[2] = gyro_bias[2] / (int32_t) fcount;

	//Serial.print("Calibration complete");
	Serial.println(gBias[0]);
	Serial.println(gBias[1]);
	Serial.println(gBias[2]);

}

void FXAS21002C::reset()
{
	writeReg(FXAS21002C_H_CTRL_REG1, 0x20); // set reset bit to 1 to assert software reset to zero at end of boot process
	delay(100);
while(!(readReg(FXAS21002C_H_INT_SRC_FLAG) & 0x08))  { // wait for boot end flag to be set
}

}
// Private Methods //////////////////////////////////////////////////////////////
<commit_msg>Delete FXAS21002C.cpp<commit_after><|endoftext|>


<commit_before>/*
    Copyright (C) 2002 by Jorrit Tyberghein

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include <stdarg.h>

#include "cssysdef.h"
#include "iengine/engine.h"
#include "iutil/objreg.h"
#include "iutil/vfs.h"
#include "cstool/initapp.h"
#include "iutil/eventq.h"
#include "csutil/cseventq.h"
#include "cssys/sysfunc.h"

//------------------------------------------------- We need the 3D engine -----

CS_IMPLEMENT_APPLICATION

//-----------------------------------------------------------------------------

bool run_pool_tests(iObjectRegistry* object_reg)
{
  csTicks begin, end;
  csRef<iEvent> ten[10], hundred[100], thousand[1000], lots[100000];

  csRef<iEventQueue> q (CS_QUERY_REGISTRY(object_reg, iEventQueue));
  if (!q)
  {
    printf ("EventQueue not loaded, Zoinks!\n");
    return false;
  }
  csEventQueue *eq = STATIC_CAST(csEventQueue *, (iEventQueue *)q);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 10 events...  ");
  begin = csGetTicks();
  int i;
  for (i = 0; i < 10; i++)
  {
    ten[i] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 100 events...  ");
  begin = csGetTicks();
  for (i = 0; i < 100; i++)
  {
    hundred[i] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 1000 events...  ");
  begin = csGetTicks();
  for (i = 0; i < 1000; i += 10)
  {
    thousand[i] = q->CreateEvent(1);
    thousand[i+1] = q->CreateEvent(1);
    thousand[i+2] = q->CreateEvent(1);
    thousand[i+3] = q->CreateEvent(1);
    thousand[i+4] = q->CreateEvent(1);
    thousand[i+5] = q->CreateEvent(1);
    thousand[i+6] = q->CreateEvent(1);
    thousand[i+7] = q->CreateEvent(1);
    thousand[i+8] = q->CreateEvent(1);
    thousand[i+9] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 10000 events...  ");
  begin = csGetTicks();
  // On my machine, this unrolled loop is 10x faster then doing one at a time
  for (i = 0; i < 10000; i += 10)
  {
    lots[i] = q->CreateEvent(1);
    lots[i+1] = q->CreateEvent(1);
    lots[i+2] = q->CreateEvent(1);
    lots[i+3] = q->CreateEvent(1);
    lots[i+4] = q->CreateEvent(1);
    lots[i+5] = q->CreateEvent(1);
    lots[i+6] = q->CreateEvent(1);
    lots[i+7] = q->CreateEvent(1);
    lots[i+8] = q->CreateEvent(1);
    lots[i+9] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  return true;
}

bool compare_buffers(char *b1, char *b2, uint32 length)
{
  for (uint32 i = 0; i < length; i++)
  {
    if (b1[i] != b2[i])
      return false;
  }
  return true;
}

bool run_event_tests(iObjectRegistry* object_reg)
{
  csRef<iEventQueue> q (CS_QUERY_REGISTRY(object_reg, iEventQueue));
  if (!q)
  {
    fprintf (stdout, "EventQueue not loaded, Zoinks!\n");
    return false;
  }

  csRef<iEvent> e = q->CreateEvent(1);
  fprintf (stdout, "Ok, now it's time to test the events themselves:\n");

  fprintf (stdout, "Adding an my_int8... %s\n", e->Add("my_int8", (int8)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint8... %s\n", e->Add("my_uint8", (uint8)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_int16... %s\n", e->Add("my_int16", (int16)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint16... %s\n", e->Add("my_uint16", (uint16)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_int32... %s\n", e->Add("my_int32", (int32)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint32... %s\n", e->Add("my_uint32", (uint32)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_int64... %s\n", e->Add("my_int64", (int64)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint64... %s\n", e->Add("my_uint64", (uint64)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_float... %s\n", e->Add("my_float", (float)1.0) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_double... %s\n", e->Add("my_double", (double)1.0) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_bool... %s\n", e->Add("my_bool", (bool)true) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_string... %s\n", e->Add("my_string", "Hello World!") ? "Sucess!" : "Failure!");
  int8 my_int8 = 0;
  fprintf (stdout, "Searching for my_int8...%s", e->Find("my_int8", my_int8) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_int8);
  uint8 my_uint8 = 0;
  fprintf (stdout, "Searching for my_uint8...%s", e->Find("my_uint8", my_uint8) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_uint8);
  int16 my_int16 = 0;
  fprintf (stdout, "Searching for my_int16...%s", e->Find("my_int16", my_int16) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_int16);
  uint16 my_uint16 = 0;
  fprintf (stdout, "Searching for my_uint16...%s", e->Find("my_uint16", my_uint16) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_uint16);
  int32 my_int32 = 0;
  fprintf (stdout, "Searching for my_int32...%s", e->Find("my_int32", my_int32) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_int32);
  uint32 my_uint32 = 0;
  fprintf (stdout, "Searching for my_uint32...%s", e->Find("my_uint32", my_uint32) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %u)\n", my_uint32);
  int64 my_int64 = 0;
  fprintf (stdout, "Searching for my_int64...%s", e->Find("my_int64", my_int64) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %lld)\n", my_int64);
  uint64 my_uint64 = 0;
  fprintf (stdout, "Searching for my_uint64...%s", e->Find("my_uint64", my_uint64) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %llu)\n", my_uint64);
  float my_float= 0;
  fprintf (stdout, "Searching for my_float...%s", e->Find("my_float", my_float) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %f)\n", my_float);
  double my_double = 0;
  fprintf (stdout, "Searching for my_double...%s", e->Find("my_double", my_double) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %f)\n", my_double);
  bool my_bool = false;
  fprintf (stdout, "Searching for my_bool...%s", e->Find("my_bool", my_bool) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %s)\n", my_bool ? "true" : "false");
  char *my_string = NULL;
  fprintf (stdout, "Searching for my_string...%s", e->Find("my_string", &my_string) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %s)\n", my_string);
  char buffer[] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
    0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f };
  fprintf (stdout, "Adding a databuffer 32 in length called my_databuffer... %s\n", e->Add("my_databuffer", (void*)buffer, 32) ? "Sucess!" : "Failure!");
  my_string = NULL;
  uint32 l;
  bool found = false;
  fprintf (stdout, "Searching for my_databuffer...%s\n", (found = e->Find("my_databuffer", (void**)&my_string, (uint32&)l, 0)) ? "Success!" : "Failure!");
  if (found)
    fprintf (stdout, "  Comparing buffers...%s\n", compare_buffers(my_string, (char*)buffer, 32) ? "Perfect!" : "Not the same!");

  fprintf (stdout, "Adding an this event to itself... %s\n", e->Add("my_event", e) ? "Sucess!" : "Failure!");
  fprintf (stdout, "  That should have failed.\n");
  csRef<iEvent> f = q->CreateEvent(1);
  fprintf (stdout, "Adding a new event as my_event... %s\n", e->Add("my_event", f) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding the parent event to the child event... %s\n", f->Add("my_event", e) ? "Sucess!" : "Failure!");
  fprintf (stdout, "  That should have failed.\n");
  uint32 size = e->FlattenSize();
  char *serialized_buffer = new char[size];
  fprintf (stdout, "Serializing the event to a buffer...%s\n", e->Flatten(serialized_buffer) ? "Sucess!" : "Failure");
  FILE *log = fopen("test.bin", "wb");
  fwrite(serialized_buffer,size, 1, log);
  fclose(log);
  csRef<iEvent> a = q->CreateEvent(1);
  fprintf (stdout, "Recreating event from the buffer...%s\n", a->Unflatten(serialized_buffer, size) ? "Sucess!" : "Failure");
  fprintf (stdout, "Printing the contents of the new event out:\n");
  a->Print();

  return true;
}

bool DoStuff (iObjectRegistry* object_reg)
{
  csRef<iEventQueue> q (CS_QUERY_REGISTRY(object_reg, iEventQueue));
  if (!q)
  {
    fprintf (stdout, "EventQueue not loaded, Zoinks!\n");
    return false;
  }
  fprintf (stdout, "\nEvent Pool test v0.96\nby Jonathan Tarbox\n\n");

  fprintf (stdout, "This will allocate 10, 100, and then 1000 events.\n");
  fprintf (stdout, "As these are the initial allocations, they will be\n");
  fprintf (stdout, "allocating memory each time, thus a little slow.\n\n");

  if (!run_pool_tests(object_reg)) return false;

  fprintf (stdout, "\nOk, now that we've left the scope of the last test\n");
  fprintf (stdout, "all the events should have DecRef'd and should be sitting\n");
  fprintf (stdout, "in the pool.  Let's try the same test again and see how\n");
  fprintf (stdout, "much faster this is.\n\n");

  if (!run_pool_tests(object_reg)) return false;

  if (!run_event_tests(object_reg)) return false;

  fprintf (stdout, "\n\nTests complete.. if it hasn't crashed yet, that's a good thing!\n");
  return true;

}

/*---------------------------------------------------------------------*
 * Main function
 *---------------------------------------------------------------------*/
int main (int argc, char* argv[])
{
  // Initialize the random number generator
  srand (time (NULL));

  iObjectRegistry* object_reg = csInitializer::CreateEnvironment (argc, argv);
  if (!object_reg)
    return -1;
  if (!csInitializer::RequestPlugins (object_reg,
  	CS_REQUEST_VFS,
	CS_REQUEST_END))
  {
    csInitializer::DestroyApplication (object_reg);
    return -1;
  }

  DoStuff (object_reg);

  csInitializer::DestroyApplication (object_reg);
  return 0;
}

<commit_msg>Eric Sunshine eliminated numerous compilation warnings throughout the project.<commit_after>/*
    Copyright (C) 2002 by Jorrit Tyberghein

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include <stdarg.h>

#include "cssysdef.h"
#include "iengine/engine.h"
#include "iutil/objreg.h"
#include "iutil/vfs.h"
#include "cstool/initapp.h"
#include "iutil/eventq.h"
#include "csutil/cseventq.h"
#include "cssys/sysfunc.h"

//------------------------------------------------- We need the 3D engine -----

CS_IMPLEMENT_APPLICATION

//-----------------------------------------------------------------------------

bool run_pool_tests(iObjectRegistry* object_reg)
{
  csTicks begin, end;
  csRef<iEvent> ten[10], hundred[100], thousand[1000], lots[100000];

  csRef<iEventQueue> q (CS_QUERY_REGISTRY(object_reg, iEventQueue));
  if (!q)
  {
    printf ("EventQueue not loaded, Zoinks!\n");
    return false;
  }
  csEventQueue *eq = STATIC_CAST(csEventQueue *, (iEventQueue *)q);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 10 events...  ");
  begin = csGetTicks();
  int i;
  for (i = 0; i < 10; i++)
  {
    ten[i] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 100 events...  ");
  begin = csGetTicks();
  for (i = 0; i < 100; i++)
  {
    hundred[i] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 1000 events...  ");
  begin = csGetTicks();
  for (i = 0; i < 1000; i += 10)
  {
    thousand[i] = q->CreateEvent(1);
    thousand[i+1] = q->CreateEvent(1);
    thousand[i+2] = q->CreateEvent(1);
    thousand[i+3] = q->CreateEvent(1);
    thousand[i+4] = q->CreateEvent(1);
    thousand[i+5] = q->CreateEvent(1);
    thousand[i+6] = q->CreateEvent(1);
    thousand[i+7] = q->CreateEvent(1);
    thousand[i+8] = q->CreateEvent(1);
    thousand[i+9] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  fprintf (stdout, "Event Pool count: %d\n", eq->CountPool());
  fprintf (stdout, "Allocating 10000 events...  ");
  begin = csGetTicks();
  // On my machine, this unrolled loop is 10x faster then doing one at a time
  for (i = 0; i < 10000; i += 10)
  {
    lots[i] = q->CreateEvent(1);
    lots[i+1] = q->CreateEvent(1);
    lots[i+2] = q->CreateEvent(1);
    lots[i+3] = q->CreateEvent(1);
    lots[i+4] = q->CreateEvent(1);
    lots[i+5] = q->CreateEvent(1);
    lots[i+6] = q->CreateEvent(1);
    lots[i+7] = q->CreateEvent(1);
    lots[i+8] = q->CreateEvent(1);
    lots[i+9] = q->CreateEvent(1);
  }
  end = csGetTicks();
  fprintf (stdout, "%d ms\n\n", end - begin);

  return true;
}

bool compare_buffers(char *b1, char *b2, uint32 length)
{
  for (uint32 i = 0; i < length; i++)
  {
    if (b1[i] != b2[i])
      return false;
  }
  return true;
}

bool run_event_tests(iObjectRegistry* object_reg)
{
  csRef<iEventQueue> q (CS_QUERY_REGISTRY(object_reg, iEventQueue));
  if (!q)
  {
    fprintf (stdout, "EventQueue not loaded, Zoinks!\n");
    return false;
  }

  csRef<iEvent> e = q->CreateEvent(1);
  fprintf (stdout, "Ok, now it's time to test the events themselves:\n");

  fprintf (stdout, "Adding an my_int8... %s\n", e->Add("my_int8", (int8)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint8... %s\n", e->Add("my_uint8", (uint8)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_int16... %s\n", e->Add("my_int16", (int16)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint16... %s\n", e->Add("my_uint16", (uint16)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_int32... %s\n", e->Add("my_int32", (int32)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint32... %s\n", e->Add("my_uint32", (uint32)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_int64... %s\n", e->Add("my_int64", (int64)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_uint64... %s\n", e->Add("my_uint64", (uint64)1) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_float... %s\n", e->Add("my_float", (float)1.0) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_double... %s\n", e->Add("my_double", (double)1.0) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_bool... %s\n", e->Add("my_bool", (bool)true) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding an my_string... %s\n", e->Add("my_string", "Hello World!") ? "Sucess!" : "Failure!");
  int8 my_int8 = 0;
  fprintf (stdout, "Searching for my_int8...%s", e->Find("my_int8", my_int8) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_int8);
  uint8 my_uint8 = 0;
  fprintf (stdout, "Searching for my_uint8...%s", e->Find("my_uint8", my_uint8) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_uint8);
  int16 my_int16 = 0;
  fprintf (stdout, "Searching for my_int16...%s", e->Find("my_int16", my_int16) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_int16);
  uint16 my_uint16 = 0;
  fprintf (stdout, "Searching for my_uint16...%s", e->Find("my_uint16", my_uint16) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_uint16);
  int32 my_int32 = 0;
  fprintf (stdout, "Searching for my_int32...%s", e->Find("my_int32", my_int32) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %d)\n", my_int32);
  uint32 my_uint32 = 0;
  fprintf (stdout, "Searching for my_uint32...%s", e->Find("my_uint32", my_uint32) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %u)\n", my_uint32);
  int64 my_int64 = 0;
  fprintf (stdout, "Searching for my_int64...%s", e->Find("my_int64", my_int64) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %lld)\n", (long long)my_int64);
  uint64 my_uint64 = 0;
  fprintf (stdout, "Searching for my_uint64...%s", e->Find("my_uint64", my_uint64) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %llu)\n", (unsigned long long)my_uint64);
  float my_float= 0;
  fprintf (stdout, "Searching for my_float...%s", e->Find("my_float", my_float) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %f)\n", my_float);
  double my_double = 0;
  fprintf (stdout, "Searching for my_double...%s", e->Find("my_double", my_double) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %f)\n", my_double);
  bool my_bool = false;
  fprintf (stdout, "Searching for my_bool...%s", e->Find("my_bool", my_bool) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %s)\n", my_bool ? "true" : "false");
  char *my_string = NULL;
  fprintf (stdout, "Searching for my_string...%s", e->Find("my_string", &my_string) ? "Success!" : "Failure!");
  fprintf (stdout, "  (value: %s)\n", my_string);
  char buffer[] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
    0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f };
  fprintf (stdout, "Adding a databuffer 32 in length called my_databuffer... %s\n", e->Add("my_databuffer", (void*)buffer, 32) ? "Sucess!" : "Failure!");
  my_string = NULL;
  uint32 l;
  bool found = false;
  fprintf (stdout, "Searching for my_databuffer...%s\n", (found = e->Find("my_databuffer", (void**)&my_string, (uint32&)l, 0)) ? "Success!" : "Failure!");
  if (found)
    fprintf (stdout, "  Comparing buffers...%s\n", compare_buffers(my_string, (char*)buffer, 32) ? "Perfect!" : "Not the same!");

  fprintf (stdout, "Adding an this event to itself... %s\n", e->Add("my_event", e) ? "Sucess!" : "Failure!");
  fprintf (stdout, "  That should have failed.\n");
  csRef<iEvent> f = q->CreateEvent(1);
  fprintf (stdout, "Adding a new event as my_event... %s\n", e->Add("my_event", f) ? "Sucess!" : "Failure!");
  fprintf (stdout, "Adding the parent event to the child event... %s\n", f->Add("my_event", e) ? "Sucess!" : "Failure!");
  fprintf (stdout, "  That should have failed.\n");
  uint32 size = e->FlattenSize();
  char *serialized_buffer = new char[size];
  fprintf (stdout, "Serializing the event to a buffer...%s\n", e->Flatten(serialized_buffer) ? "Sucess!" : "Failure");
  FILE *log = fopen("test.bin", "wb");
  fwrite(serialized_buffer,size, 1, log);
  fclose(log);
  csRef<iEvent> a = q->CreateEvent(1);
  fprintf (stdout, "Recreating event from the buffer...%s\n", a->Unflatten(serialized_buffer, size) ? "Sucess!" : "Failure");
  fprintf (stdout, "Printing the contents of the new event out:\n");
  a->Print();

  return true;
}

bool DoStuff (iObjectRegistry* object_reg)
{
  csRef<iEventQueue> q (CS_QUERY_REGISTRY(object_reg, iEventQueue));
  if (!q)
  {
    fprintf (stdout, "EventQueue not loaded, Zoinks!\n");
    return false;
  }
  fprintf (stdout, "\nEvent Pool test v0.96\nby Jonathan Tarbox\n\n");

  fprintf (stdout, "This will allocate 10, 100, and then 1000 events.\n");
  fprintf (stdout, "As these are the initial allocations, they will be\n");
  fprintf (stdout, "allocating memory each time, thus a little slow.\n\n");

  if (!run_pool_tests(object_reg)) return false;

  fprintf (stdout, "\nOk, now that we've left the scope of the last test\n");
  fprintf (stdout, "all the events should have DecRef'd and should be sitting\n");
  fprintf (stdout, "in the pool.  Let's try the same test again and see how\n");
  fprintf (stdout, "much faster this is.\n\n");

  if (!run_pool_tests(object_reg)) return false;

  if (!run_event_tests(object_reg)) return false;

  fprintf (stdout, "\n\nTests complete.. if it hasn't crashed yet, that's a good thing!\n");
  return true;

}

/*---------------------------------------------------------------------*
 * Main function
 *---------------------------------------------------------------------*/
int main (int argc, char* argv[])
{
  // Initialize the random number generator
  srand (time (NULL));

  iObjectRegistry* object_reg = csInitializer::CreateEnvironment (argc, argv);
  if (!object_reg)
    return -1;
  if (!csInitializer::RequestPlugins (object_reg,
  	CS_REQUEST_VFS,
	CS_REQUEST_END))
  {
    csInitializer::DestroyApplication (object_reg);
    return -1;
  }

  DoStuff (object_reg);

  csInitializer::DestroyApplication (object_reg);
  return 0;
}

<|endoftext|>


<commit_before>/*!
  \copyright (c) RDO-Team, 2003-2012
  \file      app/rdo_studio_mfc/src/edit/view_base.cpp
  \author      (rdo@rk9.bmstu.ru)
  \date      20.02.2003
  \brief
  \indent    4T
*/

// ---------------------------------------------------------------------------- PCH
#include "app/rdo_studio_mfc/pch/stdpch.h"
// ----------------------------------------------------------------------- INCLUDES
#include <QtCore/qtimer.h>
#include <QtCore/qcoreevent.h>
#include <boost/foreach.hpp>
// ----------------------------------------------------------------------- SYNOPSIS
#include "app/rdo_studio_mfc/src/edit/view_base.h"
#include "app/rdo_studio_mfc/src/application.h"
#include "app/rdo_studio_mfc/src/main_windows_base.h"
#include "app/rdo_studio_mfc/rdo_edit/rdoeditoredit.h"
// --------------------------------------------------------------------------------

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

using namespace rdoEditor;

// --------------------------------------------------------------------------------
// -------------------- RDOStudioEditBaseView
// --------------------------------------------------------------------------------
RDOStudioEditBaseView::Buffer::Buffer(QTimer* pTimer)
	: value (""    )
	, reset (false )
	, pTimer(pTimer)
{}

RDOStudioEditBaseView::RDOStudioEditBaseView(QWidget* pParent)
	: QWidget(pParent)
	, m_currentBuffer(0)
{
	m_thisCWnd.Attach(winId());

	m_bufferList.insert(BufferList::value_type(0, Buffer(new QTimer(this))));
	m_bufferList.insert(BufferList::value_type(1, Buffer(new QTimer(this))));
	m_bufferList.insert(BufferList::value_type(2, Buffer(new QTimer(this))));
	m_bufferList.insert(BufferList::value_type(3, Buffer(new QTimer(this))));

	BOOST_FOREACH(BufferList::value_type& buffer, m_bufferList)
	{
		connect(buffer.second.pTimer, SIGNAL(timeout()), this, SLOT(timerEvent(QTimerEvent*)));
	}

	//! todo qt
	//popupMenu.CreatePopupMenu();

	//CMenu* mainMenu = AfxGetMainWnd()->GetMenu();

	//rbool maximized = studioApp.getIMainWnd()->isMDIMaximazed();
	//int delta = maximized ? 1 : 0;

	//appendMenu( mainMenu, 4 + delta, &popupMenu );
	//popupMenu.AppendMenu( MF_SEPARATOR );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 3, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 4, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 5, &popupMenu );
	//popupMenu.AppendMenu( MF_SEPARATOR );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 10, &popupMenu );
	//popupMenu.AppendMenu( MF_SEPARATOR );
	//appendMenu( mainMenu->GetSubMenu( 2 + delta ), 0, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 2 + delta ), 3, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 2 + delta ), 1, &popupMenu );
}

RDOStudioEditBaseView::~RDOStudioEditBaseView()
{
	m_thisCWnd.Detach();

	//! todo qt
	// eraseMenu(&m_popupMenu);
}

void RDOStudioEditBaseView::restartBufTimer(ruint bufferID)
{
	BufferList::iterator it = m_bufferList.find(bufferID);
	if (it == m_bufferList.end())
		return;

	stopTimer(it->second.pTimer);
	if (studioApp.getStyle()->style_editor.buffer->canClearBuffer)
	{
		int delay = studioApp.getStyle()->style_editor.buffer->clearBufferDelay * 1000;
		if (delay < 0)
		{
			delay = 0;
		}
		it->second.pTimer->start(delay);
	}
}

void RDOStudioEditBaseView::stopTimer(QTimer* pTimer)
{
	ASSERT(pTimer);
	if (pTimer->isActive())
	{
		pTimer->stop();
	}
}

void RDOStudioEditBaseView::timerEvent(QTimerEvent* event)
{
	BOOST_FOREACH(BufferList::value_type& buffer, m_bufferList)
	{
		if (buffer.second.pTimer->timerId() == event->timerId())
		{
			buffer.second.reset = true;
			stopTimer(buffer.second.pTimer);
			break;
		}
	}
}
<commit_msg> - удалено лишнее<commit_after>/*!
  \copyright (c) RDO-Team, 2003-2012
  \file      app/rdo_studio_mfc/src/edit/view_base.cpp
  \author      (rdo@rk9.bmstu.ru)
  \date      20.02.2003
  \brief
  \indent    4T
*/

// ---------------------------------------------------------------------------- PCH
#include "app/rdo_studio_mfc/pch/stdpch.h"
// ----------------------------------------------------------------------- INCLUDES
#include <QtCore/qtimer.h>
#include <QtCore/qcoreevent.h>
#include <boost/foreach.hpp>
// ----------------------------------------------------------------------- SYNOPSIS
#include "app/rdo_studio_mfc/src/edit/view_base.h"
#include "app/rdo_studio_mfc/src/application.h"
#include "app/rdo_studio_mfc/src/main_windows_base.h"
#include "app/rdo_studio_mfc/rdo_edit/rdoeditoredit.h"
// --------------------------------------------------------------------------------

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

using namespace rdoEditor;

// --------------------------------------------------------------------------------
// -------------------- RDOStudioEditBaseView
// --------------------------------------------------------------------------------
RDOStudioEditBaseView::Buffer::Buffer(QTimer* pTimer)
	: value (""    )
	, reset (false )
	, pTimer(pTimer)
{}

RDOStudioEditBaseView::RDOStudioEditBaseView(QWidget* pParent)
	: QWidget(pParent)
	, m_currentBuffer(0)
{
	m_thisCWnd.Attach(winId());

	m_bufferList.insert(BufferList::value_type(0, Buffer(new QTimer(this))));
	m_bufferList.insert(BufferList::value_type(1, Buffer(new QTimer(this))));
	m_bufferList.insert(BufferList::value_type(2, Buffer(new QTimer(this))));
	m_bufferList.insert(BufferList::value_type(3, Buffer(new QTimer(this))));

	//! todo qt
	//popupMenu.CreatePopupMenu();

	//CMenu* mainMenu = AfxGetMainWnd()->GetMenu();

	//rbool maximized = studioApp.getIMainWnd()->isMDIMaximazed();
	//int delta = maximized ? 1 : 0;

	//appendMenu( mainMenu, 4 + delta, &popupMenu );
	//popupMenu.AppendMenu( MF_SEPARATOR );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 3, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 4, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 5, &popupMenu );
	//popupMenu.AppendMenu( MF_SEPARATOR );
	//appendMenu( mainMenu->GetSubMenu( 1 + delta ), 10, &popupMenu );
	//popupMenu.AppendMenu( MF_SEPARATOR );
	//appendMenu( mainMenu->GetSubMenu( 2 + delta ), 0, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 2 + delta ), 3, &popupMenu );
	//appendMenu( mainMenu->GetSubMenu( 2 + delta ), 1, &popupMenu );
}

RDOStudioEditBaseView::~RDOStudioEditBaseView()
{
	m_thisCWnd.Detach();

	//! todo qt
	// eraseMenu(&m_popupMenu);
}

void RDOStudioEditBaseView::restartBufTimer(ruint bufferID)
{
	BufferList::iterator it = m_bufferList.find(bufferID);
	if (it == m_bufferList.end())
		return;

	stopTimer(it->second.pTimer);
	if (studioApp.getStyle()->style_editor.buffer->canClearBuffer)
	{
		int delay = studioApp.getStyle()->style_editor.buffer->clearBufferDelay * 1000;
		if (delay < 0)
		{
			delay = 0;
		}
		it->second.pTimer->start(delay);
	}
}

void RDOStudioEditBaseView::stopTimer(QTimer* pTimer)
{
	ASSERT(pTimer);
	if (pTimer->isActive())
	{
		pTimer->stop();
	}
}

void RDOStudioEditBaseView::timerEvent(QTimerEvent* event)
{
	BOOST_FOREACH(BufferList::value_type& buffer, m_bufferList)
	{
		if (buffer.second.pTimer->timerId() == event->timerId())
		{
			buffer.second.reset = true;
			stopTimer(buffer.second.pTimer);
			break;
		}
	}
}
<|endoftext|>


<commit_before>/* ****************************************************************************
*
* FILE            diskManagerTest.cpp
*
* AUTHOR          Javier Lois
*
* DATE            December 2011
*
* DESCRIPTION
*
* unit testing of the engine library
*
*/

#include <sys/time.h>

#include "gtest/gtest.h"

#include "engine/DiskManager.h"

#include "au/ProcessStats.h"

#include "xmlparser/xmlParser.h"

// Test DiskManager's instantiation
TEST(diskManagerTest, instantiationTest) {
    ProcessStats pstats;
    unsigned long threadsBefore =  pstats.get_nthreads();

    //access instance without initialise. Should return NULL.
    EXPECT_EQ(engine::DiskManager::shared(), static_cast<engine::DiskManager*>(NULL))
              << "Uninitialized DiskManager should be null"; //using just NULL produces compilation error

    //call init() and then shared(). Should return a valid one.
    engine::DiskManager::init(3);
    ASSERT_TRUE(engine::DiskManager::shared() != static_cast<engine::DiskManager*>(NULL))
                << "DiskManager instance should not be null after instantiation";

    //check that new threads have been created
    pstats.refresh();
    unsigned long threadsAfter =  pstats.get_nthreads();
    EXPECT_TRUE(threadsAfter > threadsBefore);
}

//test void getInfo( std::ostringstream& output);
TEST(diskManagerTest, getInfoTest) {
    engine::Engine::init();
    engine::DiskManager::init(3);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);

    std::ostringstream info;
    engine::DiskManager::shared()->getInfo( info );

    //XML parsing
    XMLNode xMainNode=XMLNode::parseString(info.str().c_str(),"disk_manager");
    EXPECT_EQ(std::string(xMainNode.getChildNode("num_pending_operations").getClear().lpszValue), "1") << "Error writing pending operations tag";
    EXPECT_EQ(std::string(xMainNode.getChildNode("num_running_operations").getClear().lpszValue), "1") << "Error writing running operations tag";
    XMLNode queuedNode = xMainNode.getChildNode("queued");
    XMLNode diskOperationNode = queuedNode.getChildNode("disk_operation");
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("type").getText()), "read") << "Error writing type tag";
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("file_name").getText()), "test_filename.txt") << "Error writing file_name tag";
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("size").getText()), "1") << "Error writing size tag";
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("offset").getText()), "0") << "Error writing offset tag";

    XMLNode statisticsNode = xMainNode.getChildNode("statistics");
    XMLNode readNode = statisticsNode.getChildNode("read");
    EXPECT_TRUE(std::string(readNode.getChildNode("description").getText()).find("Currently    0 hits/s    0 B/s")) << "Error writing read statistics tag";
     EXPECT_EQ(std::string(readNode.getChildNode("rate").getClear().lpszValue), "0") << "Error writing read rate tag";
    XMLNode writeNode = statisticsNode.getChildNode("read");
    EXPECT_TRUE(std::string(writeNode.getChildNode("description").getText()).find("Currently    0 hits/s    0 B/s")) << "Error writing write statistics tag";
     EXPECT_EQ(std::string(writeNode.getChildNode("rate").getClear().lpszValue), "0") << "Error writing write rate tag";
    XMLNode totalNode = statisticsNode.getChildNode("read");
    EXPECT_TRUE(std::string(totalNode.getChildNode("description").getText()).find("Currently    0 hits/s    0 B/s")) << "Error writing total statistics tag";
     EXPECT_EQ(std::string(totalNode.getChildNode("rate").getClear().lpszValue), "0") << "Error writing total rate tag";

/*    CMarkup xmlData( info.str() );
    xmlData.FindElem("disk_manager");
    xmlData.IntoElem();
    xmlData.FindElem("num_pending_operations");
    EXPECT_EQ(xmlData.GetData(), "1") << "Error writing pending operations tag";
    xmlData.FindElem("num_running_operations");
    EXPECT_EQ(xmlData.GetData(), "1") << "Error writing running operations tag";
    //xmlData.OutOfElem();
    xmlData.FindElem("queued");
    xmlData.IntoElem();
    xmlData.FindElem("disk_operation");
    xmlData.IntoElem();
    xmlData.FindElem("type");
    EXPECT_EQ(xmlData.GetData(), "read") << "Error writing type tag";
    xmlData.FindElem("file_name");
    EXPECT_EQ(xmlData.GetData(), "test_filename.txt") << "Error writing file_name tag";
    xmlData.FindElem("size");
    EXPECT_EQ(xmlData.GetData(), "1") << "Error writing size tag";
    xmlData.FindElem("offset");
if(xmlData.GetData()!="0") std::cout << info.str() << std::endl;
    EXPECT_EQ(xmlData.GetData(), "0") << "Error writing offset tag";
    xmlData.OutOfElem();//disk_element
    xmlData.OutOfElem();//queued
    xmlData.FindElem("statistics");
    xmlData.IntoElem();
    xmlData.FindElem("read");
    xmlData.IntoElem();
    xmlData.FindElem("description");
    EXPECT_TRUE(xmlData.GetData().find("Currently    0 hits/s    0 B/s")) << "Error writing read statistics tag";
    xmlData.OutOfElem();//read
    xmlData.FindElem("write");
    xmlData.IntoElem();
    xmlData.FindElem("description");
    EXPECT_TRUE(xmlData.GetData().find("Currently    0 hits/s    0 B/s")) << "Error writing write statistics tag";
    xmlData.OutOfElem();//write
    xmlData.FindElem("total");
    xmlData.IntoElem();
    xmlData.FindElem("description");
    EXPECT_TRUE(xmlData.GetData().find("Currently    0 hits/s    0 B/s")) << "Error writing total statistics tag";
    xmlData.OutOfElem();//total
*/

}

//test void add( DiskOperation *operation )
TEST(diskManagerTest, addTest) {
    engine::Engine::init();
    engine::DiskManager::init(3);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);

    std::ostringstream info;

    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 1) << "Wrong number of disk operations";

}

//test void cancel( DiskOperation *operation );
TEST(diskManagerTest, cancelTest) {
    engine::Engine::init();
    engine::DiskManager::init(3);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);
    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 1) << "Error adding operation";
    engine::DiskManager::shared()->cancel(operation);
    usleep(50000);
    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 0) << "Operation was not cancelled";

}

//test void run_worker();  (TODO: How??)

//test int getNumOperations();
TEST(diskManagerTest, getNumOperationsTest) {
    //add two operations and check that it returns 2
    engine::Engine::init();
    engine::DiskManager::init(3);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);
    engine::DiskOperation* operation2 = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation2);

    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 2) << "Error in getNumOperations()";

}

//test void setNumOperations( int _num_disk_operations );
TEST(diskManagerTest, setNumOperationsTest) {
    engine::Engine::init();
    engine::DiskManager::init(3);

    ProcessStats pstats;
    long threadsBefore = pstats.get_nthreads();

    engine::DiskManager::shared()->setNumOperations(6);

    pstats.refresh();
    long threadsAfter = pstats.get_nthreads(); //should be threadsBefore+3, since we extended the number of operations from 3 to 6

    EXPECT_EQ(threadsAfter - threadsBefore, 3) << "Error in setNumOperations()";

}

//test void quitEngineService();
TEST(diskManagerTest, quitEngineServiceTest) {
    engine::Engine::init();
    engine::DiskManager::init(3);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);

    ProcessStats pstats;
    unsigned long threadsBefore = pstats.get_nthreads();
    engine::DiskManager::shared()->quitEngineService();
    pstats.refresh();
    unsigned long threadsAfter = pstats.get_nthreads();

    //Now the number of threads should have been reduced
    EXPECT_TRUE(threadsAfter < threadsBefore);
}

<commit_msg>More unit testing fixes<commit_after>/* ****************************************************************************
*
* FILE            diskManagerTest.cpp
*
* AUTHOR          Javier Lois
*
* DATE            December 2011
*
* DESCRIPTION
*
* unit testing of the engine library
*
*/

#include <sys/time.h>

#include "gtest/gtest.h"

#include "engine/DiskManager.h"

#include "au/ProcessStats.h"

#include "xmlparser/xmlParser.h"

// Test DiskManager's instantiation
TEST(diskManagerTest, instantiationTest) {
    ProcessStats pstats;
    unsigned long threadsBefore =  pstats.get_nthreads();

    //access instance without initialise. Should return NULL.
    EXPECT_EQ(engine::DiskManager::shared(), static_cast<engine::DiskManager*>(NULL))
              << "Uninitialized DiskManager should be null"; //using just NULL produces compilation error

    //call init() and then shared(). Should return a valid one.
    engine::DiskManager::init(10);
    ASSERT_TRUE(engine::DiskManager::shared() != static_cast<engine::DiskManager*>(NULL))
                << "DiskManager instance should not be null after instantiation";

    //check that new threads have been created
    pstats.refresh();
    unsigned long threadsAfter =  pstats.get_nthreads();
    EXPECT_TRUE(threadsAfter > threadsBefore);
}

//test void getInfo( std::ostringstream& output);
TEST(diskManagerTest, getInfoTest) {
    engine::Engine::init();
    engine::DiskManager::init(10);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);

    std::ostringstream info;
    engine::DiskManager::shared()->getInfo( info );
    //std::cout << std::endl << info.str() << std::endl;

    //XML parsing
    XMLNode xMainNode=XMLNode::parseString(info.str().c_str(),"disk_manager");
    EXPECT_EQ(std::string(xMainNode.getChildNode("num_pending_operations").getClear().lpszValue), "1") << "Error writing pending operations tag";
    EXPECT_EQ(std::string(xMainNode.getChildNode("num_running_operations").getClear().lpszValue), "1") << "Error writing running operations tag";
    XMLNode queuedNode = xMainNode.getChildNode("queued");
    ASSERT_TRUE(!queuedNode.isEmpty());
    XMLNode diskOperationNode = queuedNode.getChildNode("disk_operation");
    ASSERT_TRUE(!diskOperationNode.isEmpty());
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("type").getText()), "read") << "Error writing type tag";
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("file_name").getText()), "test_filename.txt") << "Error writing file_name tag";
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("size").getText()), "1") << "Error writing size tag";
    EXPECT_EQ(std::string(diskOperationNode.getChildNode("offset").getText()), "0") << "Error writing offset tag";

    XMLNode statisticsNode = xMainNode.getChildNode("statistics");
    XMLNode readNode = statisticsNode.getChildNode("read");
    EXPECT_TRUE(std::string(readNode.getChildNode("description").getText()).find("Currently    0 hits/s    0 B/s")) << "Error writing read statistics tag";
     EXPECT_EQ(std::string(readNode.getChildNode("rate").getClear().lpszValue), "0") << "Error writing read rate tag";
    XMLNode writeNode = statisticsNode.getChildNode("read");
    EXPECT_TRUE(std::string(writeNode.getChildNode("description").getText()).find("Currently    0 hits/s    0 B/s")) << "Error writing write statistics tag";
     EXPECT_EQ(std::string(writeNode.getChildNode("rate").getClear().lpszValue), "0") << "Error writing write rate tag";
    XMLNode totalNode = statisticsNode.getChildNode("read");
    EXPECT_TRUE(std::string(totalNode.getChildNode("description").getText()).find("Currently    0 hits/s    0 B/s")) << "Error writing total statistics tag";
     EXPECT_EQ(std::string(totalNode.getChildNode("rate").getClear().lpszValue), "0") << "Error writing total rate tag";

}

//test void add( DiskOperation *operation )
TEST(diskManagerTest, addTest) {
    engine::Engine::init();
    engine::DiskManager::init(10);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);

    std::ostringstream info;

    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 1) << "Wrong number of disk operations";

}

//test void cancel( DiskOperation *operation );
TEST(diskManagerTest, cancelTest) {
    engine::Engine::init();
    engine::DiskManager::init(10);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);
    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 1) << "Error adding operation";
    engine::DiskManager::shared()->cancel(operation);
    usleep(50000);
    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 0) << "Operation was not cancelled";

}

//test void run_worker();  (TODO: How??)

//test int getNumOperations();
TEST(diskManagerTest, getNumOperationsTest) {
    //add two operations and check that it returns 2
    engine::Engine::init();
    engine::DiskManager::init(10);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);
    engine::DiskOperation* operation2 = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation2);

    EXPECT_EQ(engine::DiskManager::shared()->getNumOperations(), 2) << "Error in getNumOperations()";

}

//test void setNumOperations( int _num_disk_operations );
TEST(diskManagerTest, setNumOperationsTest) {
    engine::Engine::init();
    engine::DiskManager::init(10);

    ProcessStats pstats;
    long threadsBefore = pstats.get_nthreads();

    engine::DiskManager::shared()->setNumOperations(14);

    pstats.refresh();
    long threadsAfter = pstats.get_nthreads(); //should be threadsBefore+4, since we extended the number of operations from 10 to 14

    EXPECT_EQ(threadsAfter - threadsBefore, 4) << "Error in setNumOperations()";

}

//test void quitEngineService();
TEST(diskManagerTest, quitEngineServiceTest) {
    engine::Engine::init();
    engine::DiskManager::init(10);
    char buffer[1024*1024];
    engine::DiskOperation* operation = engine::DiskOperation::newReadOperation( buffer , "test_filename.txt" , 0 , 1, 0 );
    engine::DiskManager::shared()->add(operation);

    ProcessStats pstats;
    unsigned long threadsBefore = pstats.get_nthreads();
    engine::DiskManager::shared()->quitEngineService();
    pstats.refresh();
    unsigned long threadsAfter = pstats.get_nthreads();

    //Now the number of threads should have been reduced
    EXPECT_TRUE(threadsAfter < threadsBefore);
}

<|endoftext|>


<commit_before>#ifndef __AXIS_HPP
#define __AXIS_HPP

#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif

enum HomingState_t {
    HOMING_STATE_IDLE,
    HOMING_STATE_HOMING,
    HOMING_STATE_MOVE_TO_ZERO
};

class Axis {
public:
    enum Error_t {
        ERROR_NONE = 0x00,
        ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
        ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
        ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
        ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08,
        ERROR_BRAKE_RESISTOR_DISARMED = 0x10, //<! the brake resistor was unexpectedly disarmed
        ERROR_MOTOR_DISARMED = 0x20, //<! the motor was unexpectedly disarmed
        ERROR_MOTOR_FAILED = 0x40, // Go to motor.hpp for information, check odrvX.axisX.motor.error for error value
        ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80,
        ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
        ERROR_CONTROLLER_FAILED = 0x200,
        ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
        ERROR_MIN_ENDSTOP_PRESSED = 0x800,
        ERROR_MAX_ENDSTOP_PRESSED = 0x1000
    };

    enum State_t {
        AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
        AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
        AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
        AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3,   //<! run all calibration procedures, then idle
        AXIS_STATE_MOTOR_CALIBRATION = 4,   //<! run motor calibration
        AXIS_STATE_SENSORLESS_CONTROL = 5,  //<! run sensorless control
        AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
        AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
        AXIS_STATE_CLOSED_LOOP_CONTROL = 8,  //<! run closed loop control
        AXIS_STATE_LOCKIN_SPIN = 9,       //<! run lockin spin
        AXIS_STATE_ENCODER_DIR_FIND = 10,
        AXIS_STATE_HOMING = 9   //<! run axis homing function

    struct LockinConfig_t {
        float current = 10.0f;           // [A]
        float ramp_time = 0.4f;          // [s]
        float ramp_distance = 1 * M_PI;  // [rad]
        float accel = 20.0f;     // [rad/s^2]
        float vel = 40.0f; // [rad/s]
        float finish_distance = 100.0f;  // [rad]
        bool finish_on_vel = false;
        bool finish_on_distance = false;
        bool finish_on_enc_idx = false;
    };

    struct Config_t {
        bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
        bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
                                                // this only has an effect if encoder.config.use_index is also true
        bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
        bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
        bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
        bool startup_homing = false; //<! enable homing after calibration/startup
        bool enable_step_dir = false; //<! enable step/dir input after calibration
                                    //   For M0 this has no effect if enable_uart is true
        float counts_per_step = 2.0f;

        float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)

        // Defaults loaded from hw_config in load_configuration in main.cpp
        uint16_t step_gpio_pin = 0;
        uint16_t dir_gpio_pin = 0;

        LockinConfig_t lockin;
    };

    enum thread_signals {
        M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
    };

    enum LockinState_t {
        LOCKIN_STATE_INACTIVE,
        LOCKIN_STATE_RAMP,
        LOCKIN_STATE_ACCELERATE,
        LOCKIN_STATE_CONST_VEL,
    };

    Axis(const AxisHardwareConfig_t& hw_config,
            Config_t& config,
            Encoder& encoder,
            SensorlessEstimator& sensorless_estimator,
            Controller& controller,
            Motor& motor,
            TrapezoidalTrajectory& trap,
            Endstop& min_endstop,
            Endstop& max_endstop);

    void setup();
    void start_thread();
    void signal_current_meas();
    bool wait_for_current_meas();

    void step_cb();
    void set_step_dir_active(bool enable);
    void decode_step_dir_pins();
    void update_watchdog_settings();

    static void load_default_step_dir_pin_config(
        const AxisHardwareConfig_t& hw_config, Config_t* config);

    bool check_DRV_fault();
    bool check_PSU_brownout();
    bool do_checks();
    bool do_updates();

    void watchdog_feed();
    bool watchdog_check();

    // True if there are no errors
    bool inline check_for_errors() {
        return error_ == ERROR_NONE;
    }

    // @brief Runs the specified update handler at the frequency of the current measurements.
    //
    // The loop runs until one of the following conditions:
    //  - update_handler returns false
    //  - the current measurement times out
    //  - the health checks fail (brownout, driver fault line)
    //  - update_handler doesn't update the modulation timings in time
    //    This criterion is ignored if current_state is AXIS_STATE_IDLE
    //
    // If update_handler is going to update the motor timings, you must call motor.arm()
    // shortly before this function.
    //
    // If the function returns, it is guaranteed that error is non-zero, except if the cause
    // for the exit was a negative return value of update_handler or an external
    // state change request (requested_state != AXIS_STATE_DONT_CARE).
    // Under all exit conditions the motor is disarmed and the brake current set to zero.
    // Furthermore, if the update_handler does not set the phase voltages in time, they will
    // go to zero.
    //
    // @tparam T Must be a callable type that takes no arguments and returns a bool
    template<typename T>
    void run_control_loop(const T& update_handler) {
        while (requested_state_ == AXIS_STATE_UNDEFINED) {
            // look for errors at axis level and also all subcomponents
            bool checks_ok = do_checks();
            // Update all estimators
            // Note: updates run even if checks fail
            bool updates_ok = do_updates();

            // make sure the watchdog is being fed.
            bool watchdog_ok = watchdog_check();

            if (!checks_ok || !updates_ok || !watchdog_ok) {
                // It's not useful to quit idle since that is the safe action
                // Also leaving idle would rearm the motors
                if (current_state_ != AXIS_STATE_IDLE)
                    break;
            }

            // Run main loop function, defer quitting for after wait
            // TODO: change arming logic to arm after waiting
            bool main_continue = update_handler();

            // Check we meet deadlines after queueing
            ++loop_counter_;

            // Wait until the current measurement interrupt fires
            if (!wait_for_current_meas()) {
                // maybe the interrupt handler is dead, let's be
                // safe and float the phases
                safety_critical_disarm_motor_pwm(motor_);
                update_brake_current();
                error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT;
                break;
            }

            if (!main_continue)
                break;
        }
    }

    bool run_lockin_spin();
    bool run_sensorless_control_loop();
    bool run_closed_loop_control_loop();
    bool run_idle_loop();

    void run_state_machine_loop();

    const AxisHardwareConfig_t& hw_config_;
    Config_t& config_;

    Encoder& encoder_;
    SensorlessEstimator& sensorless_estimator_;
    Controller& controller_;
    Motor& motor_;
    TrapezoidalTrajectory& trap_;
    Endstop& min_endstop_;
    Endstop& max_endstop_;

    osThreadId thread_id_;
    volatile bool thread_id_valid_ = false;

    // variables exposed on protocol
    Error_t error_ = ERROR_NONE;
    bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir

    // updated from config in constructor, and on protocol hook
    GPIO_TypeDef* step_port_;
    uint16_t step_pin_;
    GPIO_TypeDef* dir_port_;
    uint16_t dir_pin_;

    State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
    State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
    State_t& current_state_ = task_chain_[0];
    uint32_t loop_counter_ = 0;
    LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
    HomingState_t homing_state_ = HOMING_STATE_IDLE;

    // watchdog
    uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings()
    uint32_t watchdog_current_value_= 0;

    // Communication protocol definitions
    auto make_protocol_definitions() {
        return make_protocol_member_list(
            make_protocol_property("error", &error_),
            make_protocol_ro_property("step_dir_active", &step_dir_active_),
            make_protocol_ro_property("current_state", &current_state_),
            make_protocol_property("requested_state", &requested_state_),
            make_protocol_ro_property("loop_counter", &loop_counter_),
            make_protocol_ro_property("lockin_state", &lockin_state_),
            make_protocol_ro_property("homing_state", &homing_state_),
            make_protocol_object("config",
                make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
                make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
                make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration),
                make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
                make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
                make_protocol_property("startup_homing", &config_.startup_homing),
                make_protocol_property("enable_step_dir", &config_.enable_step_dir),
                make_protocol_property("counts_per_step", &config_.counts_per_step),
                make_protocol_property("watchdog_timeout", &config_.watchdog_timeout,
                    [](void* ctx) { static_cast<Axis*>(ctx)->update_watchdog_settings(); }, this),
                make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
                    [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
                    [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                make_protocol_object("lockin",
                    make_protocol_property("current", &config_.lockin.current),
                    make_protocol_property("ramp_time", &config_.lockin.ramp_time),
                    make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
                    make_protocol_property("accel", &config_.lockin.accel),
                    make_protocol_property("vel", &config_.lockin.vel),
                    make_protocol_property("finish_distance", &config_.lockin.finish_distance),
                    make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
                    make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
                    make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
                )
            ),
            make_protocol_object("motor", motor_.make_protocol_definitions()),
            make_protocol_object("controller", controller_.make_protocol_definitions()),
            make_protocol_object("encoder", encoder_.make_protocol_definitions()),
            make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
            make_protocol_object("trap_traj", trap_.make_protocol_definitions()),
            make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()),
            make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()),
            make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed)
        );
    }
};

DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t)

#endif /* __AXIS_HPP */
<commit_msg>Fix bad struct and enums<commit_after>#ifndef __AXIS_HPP
#define __AXIS_HPP

#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif

enum HomingState_t {
    HOMING_STATE_IDLE,
    HOMING_STATE_HOMING,
    HOMING_STATE_MOVE_TO_ZERO
};

class Axis {
public:
    enum Error_t {
        ERROR_NONE = 0x00,
        ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
        ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
        ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
        ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08,
        ERROR_BRAKE_RESISTOR_DISARMED = 0x10, //<! the brake resistor was unexpectedly disarmed
        ERROR_MOTOR_DISARMED = 0x20, //<! the motor was unexpectedly disarmed
        ERROR_MOTOR_FAILED = 0x40, // Go to motor.hpp for information, check odrvX.axisX.motor.error for error value
        ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80,
        ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
        ERROR_CONTROLLER_FAILED = 0x200,
        ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
        ERROR_MIN_ENDSTOP_PRESSED = 0x1000,
        ERROR_MAX_ENDSTOP_PRESSED = 0x2000
    };

    enum State_t {
        AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
        AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
        AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
        AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3,   //<! run all calibration procedures, then idle
        AXIS_STATE_MOTOR_CALIBRATION = 4,   //<! run motor calibration
        AXIS_STATE_SENSORLESS_CONTROL = 5,  //<! run sensorless control
        AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
        AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
        AXIS_STATE_CLOSED_LOOP_CONTROL = 8,  //<! run closed loop control
        AXIS_STATE_LOCKIN_SPIN = 9,       //<! run lockin spin
        AXIS_STATE_ENCODER_DIR_FIND = 10,
        AXIS_STATE_HOMING = 11,   //<! run axis homing function
    };

    struct LockinConfig_t {
        float current = 10.0f;           // [A]
        float ramp_time = 0.4f;          // [s]
        float ramp_distance = 1 * M_PI;  // [rad]
        float accel = 20.0f;     // [rad/s^2]
        float vel = 40.0f; // [rad/s]
        float finish_distance = 100.0f;  // [rad]
        bool finish_on_vel = false;
        bool finish_on_distance = false;
        bool finish_on_enc_idx = false;
    };

    struct Config_t {
        bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
        bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
                                                // this only has an effect if encoder.config.use_index is also true
        bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
        bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
        bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
        bool startup_homing = false; //<! enable homing after calibration/startup
        bool enable_step_dir = false; //<! enable step/dir input after calibration
                                    //   For M0 this has no effect if enable_uart is true
        float counts_per_step = 2.0f;

        float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)

        // Defaults loaded from hw_config in load_configuration in main.cpp
        uint16_t step_gpio_pin = 0;
        uint16_t dir_gpio_pin = 0;

        LockinConfig_t lockin;
    };

    enum thread_signals {
        M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
    };

    enum LockinState_t {
        LOCKIN_STATE_INACTIVE,
        LOCKIN_STATE_RAMP,
        LOCKIN_STATE_ACCELERATE,
        LOCKIN_STATE_CONST_VEL,
    };

    Axis(const AxisHardwareConfig_t& hw_config,
            Config_t& config,
            Encoder& encoder,
            SensorlessEstimator& sensorless_estimator,
            Controller& controller,
            Motor& motor,
            TrapezoidalTrajectory& trap,
            Endstop& min_endstop,
            Endstop& max_endstop);

    void setup();
    void start_thread();
    void signal_current_meas();
    bool wait_for_current_meas();

    void step_cb();
    void set_step_dir_active(bool enable);
    void decode_step_dir_pins();
    void update_watchdog_settings();

    static void load_default_step_dir_pin_config(
        const AxisHardwareConfig_t& hw_config, Config_t* config);

    bool check_DRV_fault();
    bool check_PSU_brownout();
    bool do_checks();
    bool do_updates();

    void watchdog_feed();
    bool watchdog_check();

    // True if there are no errors
    bool inline check_for_errors() {
        return error_ == ERROR_NONE;
    }

    // @brief Runs the specified update handler at the frequency of the current measurements.
    //
    // The loop runs until one of the following conditions:
    //  - update_handler returns false
    //  - the current measurement times out
    //  - the health checks fail (brownout, driver fault line)
    //  - update_handler doesn't update the modulation timings in time
    //    This criterion is ignored if current_state is AXIS_STATE_IDLE
    //
    // If update_handler is going to update the motor timings, you must call motor.arm()
    // shortly before this function.
    //
    // If the function returns, it is guaranteed that error is non-zero, except if the cause
    // for the exit was a negative return value of update_handler or an external
    // state change request (requested_state != AXIS_STATE_DONT_CARE).
    // Under all exit conditions the motor is disarmed and the brake current set to zero.
    // Furthermore, if the update_handler does not set the phase voltages in time, they will
    // go to zero.
    //
    // @tparam T Must be a callable type that takes no arguments and returns a bool
    template<typename T>
    void run_control_loop(const T& update_handler) {
        while (requested_state_ == AXIS_STATE_UNDEFINED) {
            // look for errors at axis level and also all subcomponents
            bool checks_ok = do_checks();
            // Update all estimators
            // Note: updates run even if checks fail
            bool updates_ok = do_updates();

            // make sure the watchdog is being fed.
            bool watchdog_ok = watchdog_check();

            if (!checks_ok || !updates_ok || !watchdog_ok) {
                // It's not useful to quit idle since that is the safe action
                // Also leaving idle would rearm the motors
                if (current_state_ != AXIS_STATE_IDLE)
                    break;
            }

            // Run main loop function, defer quitting for after wait
            // TODO: change arming logic to arm after waiting
            bool main_continue = update_handler();

            // Check we meet deadlines after queueing
            ++loop_counter_;

            // Wait until the current measurement interrupt fires
            if (!wait_for_current_meas()) {
                // maybe the interrupt handler is dead, let's be
                // safe and float the phases
                safety_critical_disarm_motor_pwm(motor_);
                update_brake_current();
                error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT;
                break;
            }

            if (!main_continue)
                break;
        }
    }

    bool run_lockin_spin();
    bool run_sensorless_control_loop();
    bool run_closed_loop_control_loop();
    bool run_idle_loop();

    void run_state_machine_loop();

    const AxisHardwareConfig_t& hw_config_;
    Config_t& config_;

    Encoder& encoder_;
    SensorlessEstimator& sensorless_estimator_;
    Controller& controller_;
    Motor& motor_;
    TrapezoidalTrajectory& trap_;
    Endstop& min_endstop_;
    Endstop& max_endstop_;

    osThreadId thread_id_;
    volatile bool thread_id_valid_ = false;

    // variables exposed on protocol
    Error_t error_ = ERROR_NONE;
    bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir

    // updated from config in constructor, and on protocol hook
    GPIO_TypeDef* step_port_;
    uint16_t step_pin_;
    GPIO_TypeDef* dir_port_;
    uint16_t dir_pin_;

    State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
    State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
    State_t& current_state_ = task_chain_[0];
    uint32_t loop_counter_ = 0;
    LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
    HomingState_t homing_state_ = HOMING_STATE_IDLE;

    // watchdog
    uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings()
    uint32_t watchdog_current_value_= 0;

    // Communication protocol definitions
    auto make_protocol_definitions() {
        return make_protocol_member_list(
            make_protocol_property("error", &error_),
            make_protocol_ro_property("step_dir_active", &step_dir_active_),
            make_protocol_ro_property("current_state", &current_state_),
            make_protocol_property("requested_state", &requested_state_),
            make_protocol_ro_property("loop_counter", &loop_counter_),
            make_protocol_ro_property("lockin_state", &lockin_state_),
            make_protocol_ro_property("homing_state", &homing_state_),
            make_protocol_object("config",
                make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
                make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
                make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration),
                make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
                make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
                make_protocol_property("startup_homing", &config_.startup_homing),
                make_protocol_property("enable_step_dir", &config_.enable_step_dir),
                make_protocol_property("counts_per_step", &config_.counts_per_step),
                make_protocol_property("watchdog_timeout", &config_.watchdog_timeout,
                    [](void* ctx) { static_cast<Axis*>(ctx)->update_watchdog_settings(); }, this),
                make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
                    [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
                    [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                make_protocol_object("lockin",
                    make_protocol_property("current", &config_.lockin.current),
                    make_protocol_property("ramp_time", &config_.lockin.ramp_time),
                    make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
                    make_protocol_property("accel", &config_.lockin.accel),
                    make_protocol_property("vel", &config_.lockin.vel),
                    make_protocol_property("finish_distance", &config_.lockin.finish_distance),
                    make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
                    make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
                    make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
                )
            ),
            make_protocol_object("motor", motor_.make_protocol_definitions()),
            make_protocol_object("controller", controller_.make_protocol_definitions()),
            make_protocol_object("encoder", encoder_.make_protocol_definitions()),
            make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
            make_protocol_object("trap_traj", trap_.make_protocol_definitions()),
            make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()),
            make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()),
            make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed)
        );
    }
};

DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t)

#endif /* __AXIS_HPP */
<|endoftext|>