<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
*/
//osgManipulator - Copyright (C) 2007 Fugro-Jason B.V.

#include <osgManipulator/Constraint>
#include <osgManipulator/Command>
#include <osgManipulator/Dragger>
#include <osg/Vec2d>

#include <math.h>

using namespace osgManipulator;

namespace
{

double round_to_nearest_int(double x) { return floor(x+0.5); }

osg::Vec3d snap_point_to_grid(const osg::Vec3d& point, const osg::Vec3d& origin, const osg::Vec3d& spacing)
{
    osg::Vec3d scale;
    scale[0] = spacing[0] ? round_to_nearest_int((point[0] - origin[0]) / spacing[0]) : 1.0;
    scale[1] = spacing[1] ? round_to_nearest_int((point[1] - origin[1]) / spacing[1]) : 1.0;
    scale[2] = spacing[2] ? round_to_nearest_int((point[2] - origin[2]) / spacing[2]) : 1.0;
    osg::Vec3d snappedPoint = origin;
    snappedPoint += osg::Vec3(scale[0]*spacing[0],scale[1]*spacing[1],scale[2]*spacing[2]);
    return snappedPoint;
}

}

void Constraint::computeLocalToWorldAndWorldToLocal() const
{
    if (!_refNode)
    {
        _localToWorld.makeIdentity();
        _worldToLocal.makeIdentity();
        return;
    }

    osg::NodePath pathToRoot;
    computeNodePathToRoot(const_cast<osg::Node&>(getReferenceNode()),pathToRoot);
    _localToWorld = osg::computeLocalToWorld(pathToRoot);
    _worldToLocal = osg::computeWorldToLocal(pathToRoot);
}

GridConstraint::GridConstraint(osg::Node& refNode, const osg::Vec3d& origin, const osg::Vec3d& spacing)
    : Constraint(refNode), _origin(origin), _spacing(spacing)
{
}

bool GridConstraint::constrain(TranslateInLineCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    osg::Vec3d translatedPoint = command.getLineStart() + command.getTranslation();
    osg::Vec3d localTranslatedPoint = (osg::Vec3d(translatedPoint)
                                       * command.getLocalToWorld() * getWorldToLocal());
    osg::Vec3d newLocalTranslatedPoint = snap_point_to_grid(localTranslatedPoint,
                                                            _origin,
                                                            _spacing);
    command.setTranslation(newLocalTranslatedPoint * getLocalToWorld() * command.getWorldToLocal() - command.getLineStart());

    return true;
}

bool GridConstraint::constrain(TranslateInPlaneCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    osg::Matrix commandToConstraint = command.getLocalToWorld() * getWorldToLocal();
    osg::Matrix constraintToCommand = getLocalToWorld() * command.getWorldToLocal();

    // Snap the reference point to grid.
    osg::Vec3d localRefPoint = command.getReferencePoint() * commandToConstraint;
    osg::Vec3d snappedLocalRefPoint = snap_point_to_grid(localRefPoint, _origin, _spacing);
    osg::Vec3d snappedCmdRefPoint = snappedLocalRefPoint * constraintToCommand;

    // Snap the translated point to grid.
    osg::Vec3d translatedPoint = snappedCmdRefPoint + command.getTranslation();
    osg::Vec3d localTranslatedPoint = osg::Vec3d(translatedPoint) * commandToConstraint;
    osg::Vec3d newLocalTranslatedPoint = snap_point_to_grid(localTranslatedPoint, _origin, _spacing);

    // Set the snapped translation.
    command.setTranslation(newLocalTranslatedPoint * constraintToCommand - snappedCmdRefPoint);

    return true;
}

bool GridConstraint::constrain(Scale1DCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    double scaledPoint = (command.getReferencePoint() - command.getScaleCenter()) * command.getScale() + command.getScaleCenter();

    osg::Matrix constraintToCommand = getLocalToWorld() * command.getWorldToLocal();
    osg::Vec3d commandOrigin = _origin * constraintToCommand;
    osg::Vec3d commandSpacing = (_origin + _spacing) * constraintToCommand - commandOrigin;

    double spacingFactor = commandSpacing[0] ? round_to_nearest_int((scaledPoint-commandOrigin[0])/commandSpacing[0]) : 1.0;

    double snappedScaledPoint = commandOrigin[0] + commandSpacing[0] * spacingFactor;

    double denom = (command.getReferencePoint() - command.getScaleCenter());
    double snappedScale = (denom) ? (snappedScaledPoint - command.getScaleCenter()) / denom : 1.0;
    if (snappedScale < command.getMinScale()) snappedScale = command.getMinScale();

    command.setScale(snappedScale);
    return true;
}

bool GridConstraint::constrain(Scale2DCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    osg::Vec2d scaledPoint = command.getReferencePoint() - command.getScaleCenter();
    scaledPoint[0] *= command.getScale()[0];
    scaledPoint[1] *= command.getScale()[1];
    scaledPoint += command.getScaleCenter();

    osg::Matrix constraintToCommand = getLocalToWorld() * command.getWorldToLocal();
    osg::Vec3d commandOrigin = _origin * constraintToCommand;
    osg::Vec3d commandSpacing = (_origin + _spacing) * constraintToCommand - commandOrigin;

    osg::Vec2d spacingFactor;
    spacingFactor[0] = commandSpacing[0] ? round_to_nearest_int((scaledPoint[0] - commandOrigin[0])/commandSpacing[0]) : 1.0;
    spacingFactor[1] = commandSpacing[2] ? round_to_nearest_int((scaledPoint[1] - commandOrigin[2])/commandSpacing[2]) : 1.0;

    osg::Vec2d snappedScaledPoint = (osg::Vec2d(commandOrigin[0],commandOrigin[2])
                                     + osg::Vec2d(commandSpacing[0]*spacingFactor[0],
                                                  commandSpacing[2]*spacingFactor[1]));

    osg::Vec2d denom = command.getReferencePoint() - command.getScaleCenter();
    osg::Vec2d snappedScale;
    snappedScale[0] = denom[0] ? (snappedScaledPoint[0] - command.getScaleCenter()[0]) / denom[0] : 1.0;
    snappedScale[1] = denom[1] ? (snappedScaledPoint[1] - command.getScaleCenter()[1]) / denom[1] : 1.0;

    if (snappedScale[0] < command.getMinScale()[0]) snappedScale[0] = command.getMinScale()[0];
    if (snappedScale[1] < command.getMinScale()[1]) snappedScale[1] = command.getMinScale()[1];

    command.setScale(snappedScale);
    return true;
}

bool GridConstraint::constrain(ScaleUniformCommand&) const
{
    // Can you correctly snap a ScaleUniformCommand using a Grid constraint that has
    // different spacings in the three axis??
    return false;
}
<commit_msg>osgManipulator: Constraint: snap_point_to_grid wants Vec3d instead of Vec3<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
*/
//osgManipulator - Copyright (C) 2007 Fugro-Jason B.V.

#include <osgManipulator/Constraint>
#include <osgManipulator/Command>
#include <osgManipulator/Dragger>
#include <osg/Vec2d>

#include <math.h>

using namespace osgManipulator;

namespace
{

double round_to_nearest_int(double x) { return floor(x+0.5); }

osg::Vec3d snap_point_to_grid(const osg::Vec3d& point, const osg::Vec3d& origin, const osg::Vec3d& spacing)
{
    osg::Vec3d scale;
    scale[0] = spacing[0] ? round_to_nearest_int((point[0] - origin[0]) / spacing[0]) : 1.0;
    scale[1] = spacing[1] ? round_to_nearest_int((point[1] - origin[1]) / spacing[1]) : 1.0;
    scale[2] = spacing[2] ? round_to_nearest_int((point[2] - origin[2]) / spacing[2]) : 1.0;
    osg::Vec3d snappedPoint = origin;
    snappedPoint += osg::Vec3d(scale[0]*spacing[0],scale[1]*spacing[1],scale[2]*spacing[2]);
    return snappedPoint;
}

}

void Constraint::computeLocalToWorldAndWorldToLocal() const
{
    if (!_refNode)
    {
        _localToWorld.makeIdentity();
        _worldToLocal.makeIdentity();
        return;
    }

    osg::NodePath pathToRoot;
    computeNodePathToRoot(const_cast<osg::Node&>(getReferenceNode()),pathToRoot);
    _localToWorld = osg::computeLocalToWorld(pathToRoot);
    _worldToLocal = osg::computeWorldToLocal(pathToRoot);
}

GridConstraint::GridConstraint(osg::Node& refNode, const osg::Vec3d& origin, const osg::Vec3d& spacing)
    : Constraint(refNode), _origin(origin), _spacing(spacing)
{
}

bool GridConstraint::constrain(TranslateInLineCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    osg::Vec3d translatedPoint = command.getLineStart() + command.getTranslation();
    osg::Vec3d localTranslatedPoint = (osg::Vec3d(translatedPoint)
                                       * command.getLocalToWorld() * getWorldToLocal());
    osg::Vec3d newLocalTranslatedPoint = snap_point_to_grid(localTranslatedPoint,
                                                            _origin,
                                                            _spacing);
    command.setTranslation(newLocalTranslatedPoint * getLocalToWorld() * command.getWorldToLocal() - command.getLineStart());

    return true;
}

bool GridConstraint::constrain(TranslateInPlaneCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    osg::Matrix commandToConstraint = command.getLocalToWorld() * getWorldToLocal();
    osg::Matrix constraintToCommand = getLocalToWorld() * command.getWorldToLocal();

    // Snap the reference point to grid.
    osg::Vec3d localRefPoint = command.getReferencePoint() * commandToConstraint;
    osg::Vec3d snappedLocalRefPoint = snap_point_to_grid(localRefPoint, _origin, _spacing);
    osg::Vec3d snappedCmdRefPoint = snappedLocalRefPoint * constraintToCommand;

    // Snap the translated point to grid.
    osg::Vec3d translatedPoint = snappedCmdRefPoint + command.getTranslation();
    osg::Vec3d localTranslatedPoint = osg::Vec3d(translatedPoint) * commandToConstraint;
    osg::Vec3d newLocalTranslatedPoint = snap_point_to_grid(localTranslatedPoint, _origin, _spacing);

    // Set the snapped translation.
    command.setTranslation(newLocalTranslatedPoint * constraintToCommand - snappedCmdRefPoint);

    return true;
}

bool GridConstraint::constrain(Scale1DCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    double scaledPoint = (command.getReferencePoint() - command.getScaleCenter()) * command.getScale() + command.getScaleCenter();

    osg::Matrix constraintToCommand = getLocalToWorld() * command.getWorldToLocal();
    osg::Vec3d commandOrigin = _origin * constraintToCommand;
    osg::Vec3d commandSpacing = (_origin + _spacing) * constraintToCommand - commandOrigin;

    double spacingFactor = commandSpacing[0] ? round_to_nearest_int((scaledPoint-commandOrigin[0])/commandSpacing[0]) : 1.0;

    double snappedScaledPoint = commandOrigin[0] + commandSpacing[0] * spacingFactor;

    double denom = (command.getReferencePoint() - command.getScaleCenter());
    double snappedScale = (denom) ? (snappedScaledPoint - command.getScaleCenter()) / denom : 1.0;
    if (snappedScale < command.getMinScale()) snappedScale = command.getMinScale();

    command.setScale(snappedScale);
    return true;
}

bool GridConstraint::constrain(Scale2DCommand& command) const
{
    if (command.getStage() == osgManipulator::MotionCommand::START)
        computeLocalToWorldAndWorldToLocal();
    else if (command.getStage() == osgManipulator::MotionCommand::FINISH)
        return true;

    osg::Vec2d scaledPoint = command.getReferencePoint() - command.getScaleCenter();
    scaledPoint[0] *= command.getScale()[0];
    scaledPoint[1] *= command.getScale()[1];
    scaledPoint += command.getScaleCenter();

    osg::Matrix constraintToCommand = getLocalToWorld() * command.getWorldToLocal();
    osg::Vec3d commandOrigin = _origin * constraintToCommand;
    osg::Vec3d commandSpacing = (_origin + _spacing) * constraintToCommand - commandOrigin;

    osg::Vec2d spacingFactor;
    spacingFactor[0] = commandSpacing[0] ? round_to_nearest_int((scaledPoint[0] - commandOrigin[0])/commandSpacing[0]) : 1.0;
    spacingFactor[1] = commandSpacing[2] ? round_to_nearest_int((scaledPoint[1] - commandOrigin[2])/commandSpacing[2]) : 1.0;

    osg::Vec2d snappedScaledPoint = (osg::Vec2d(commandOrigin[0],commandOrigin[2])
                                     + osg::Vec2d(commandSpacing[0]*spacingFactor[0],
                                                  commandSpacing[2]*spacingFactor[1]));

    osg::Vec2d denom = command.getReferencePoint() - command.getScaleCenter();
    osg::Vec2d snappedScale;
    snappedScale[0] = denom[0] ? (snappedScaledPoint[0] - command.getScaleCenter()[0]) / denom[0] : 1.0;
    snappedScale[1] = denom[1] ? (snappedScaledPoint[1] - command.getScaleCenter()[1]) / denom[1] : 1.0;

    if (snappedScale[0] < command.getMinScale()[0]) snappedScale[0] = command.getMinScale()[0];
    if (snappedScale[1] < command.getMinScale()[1]) snappedScale[1] = command.getMinScale()[1];

    command.setScale(snappedScale);
    return true;
}

bool GridConstraint::constrain(ScaleUniformCommand&) const
{
    // Can you correctly snap a ScaleUniformCommand using a Grid constraint that has
    // different spacings in the three axis??
    return false;
}
<|endoftext|>


<commit_before>//
// Created by andreanistico on 06/02/17.
//

#include <iostream>
#include "include/Lander/StatesClasses.hpp"
#include "Lander/Lander.h"
#include "parameters.h"

Lander::Lander()
        : _horizontaErr((double)0)    , _tauHold((double)0), _tauLost((double)0), _tauErr((double)0), _NHold(0),
          _NLost(0),_NComp(0), _initS(&_machine), _holdS(&_machine)  , _asceS(&_machine)  , _descS(&_machine),_compS(&_machine),
          _rtolS(&_machine),_landS(&_machine), _err(0,0,0), _err_int(0,0,0), _err_diff(0,0,0), _dt(0), _prevTime(0), _actualTime(0),_actualState(0),_prevState(0),
          _verticalErr(0)
{

    initStateMachine();
    _actualState = _machine.getActualNodeId();
    _err_prev = _err;

}

void Lander::setState(MavState pose) {
    _state = pose;
}

MavState Lander::getState() {
    return _state;
}

MavState Lander::getCommand() {
    return _setPoint;
}

void Lander::initStateMachine() {

    //Link signals
    _machine._horizontaErr =  &_horizontaErr;
    _machine._verticalErr  =  &_verticalErr;
    _machine._tauErr       =  &_tauErr;
    _machine._tauHold      =  &_tauHold;
    _machine._tauLost      =  &_tauLost;
    _machine._NLost        =  &_NLost;
    _machine._NHold        =  &_NHold;
    _machine._NComp        =  &_NComp;
    _machine._state        =  &_state;
    _machine._setPoint     =  &_setPoint;

    //Link states
    _initS._nextState    = &_holdS;
    _holdS._nextAscState = &_asceS;
    _holdS._nextDesState = &_descS;
    _holdS._nextState    = &_rtolS;
    _asceS._nextState    = &_holdS;
    _descS._nextState    = &_holdS;
    _rtolS._nextComState = &_compS;
    _rtolS._nextState    = &_holdS;
    _compS._nextState    = &_asceS;
    _compS._nextLanState = &_landS;
    _landS._nextState    = &_asceS;

    _machine.setStatePtr(&_initS);

    _tauHold = 0.5 * params_automatic::platformLenght;
    _tauLost = params_automatic::platformLenght * 0.7;

    //Print actual state
    std::cout << "Actual state: " << _machine.getActualNodeId() << std::endl;

}

void Lander::setPlatformState(const MavState platformState) {
    _platformState = platformState;
}

void Lander::updateSignals() {

    //Compute horizontal error
    double xTemp = _state.getX();
    double yTemp = _state.getY();
    double zTemp = _state.getZ();
    double xPlatTemp = _platformState.getX();
    double yPlatTemp = _platformState.getY();
    double zPlatTemp = _platformState.getZ();

    double dx = xPlatTemp - xTemp;
    double dy = yPlatTemp - yTemp;
    double dz = zPlatTemp - zTemp + PLATFORM_OFFSET;

    _err[0] = dx;
    _err[1] = dy;
    _err[2] = 0; //Trick, used to calculate horizontal error

    _horizontaErr = _err.norm();
    _err[2]       = dz;
    _verticalErr  = _err[2];

    if( _actualState == AbstractLandState::states::HOLD ||
        _actualState == AbstractLandState::states::ASCE ||
        _actualState == AbstractLandState::states::DESC ||
        _actualState == AbstractLandState::states::COMP ||
        _actualState == AbstractLandState::states::R2LA ){

        //Increment N if needed
        if (_horizontaErr < _tauHold) {
            _NLost = 0;
            _NHold++;
        }
        else if (_horizontaErr > _tauLost) {
            _NHold = 0;
            _NComp = 0;
            _NLost++;
        }

        if(_actualState == AbstractLandState::states::R2LA || _actualState == AbstractLandState::states::COMP){

            if(_prevState == AbstractLandState::states::HOLD) _NComp = 0;
            //Check whether we are on place to land
            if (_horizontaErr < _tauHold * 0.5) {
                _NComp++;
            } else{
                _NComp = 0;
            }
        }

        _err_prev = _err;
    }

#ifdef DEBUG

    std::cout << "**********************************" << std::endl;
    std::cout << "STATE: " << _actualState<< std::endl;
    std::cout << "HERRO: " << _horizontaErr<< std::endl;
    std::cout << "HERRV: " << _verticalErr<< std::endl;
    std::cout << "NHOLD: " << _NHold<< std::endl;
    std::cout << "NLOST: " << _NLost<< std::endl;
    std::cout << "NCOMP: " << _NComp<< std::endl;
    std::cout << "INTEX: " << _err_int[0] << std::endl;
    std::cout << "INTEY: " << _err_int[1] << std::endl;
    std::cout << "**********************************" << std::endl;
#endif

}

void Lander::handleMachine() {

    updateSignals();
    _machine.handle();
}

int Lander::getActualMachineState() {
    return _machine.getActualNodeId();
}

void Lander::run() {

    handleMachine();
    _actualState = _machine.getActualNodeId();
    managetime();

    static bool initDone = false;
    switch (_actualState){

        case (AbstractLandState::states::INIT):
            if(!initDone){
                init();
                initDone = true;
            }
            break;

        case (AbstractLandState::states::HOLD):
            initDone = false;
            clampZSP();
            hold();
            break;
        case (AbstractLandState::states::DESC):

            desc();
            clampZSP();
            break;
        case (AbstractLandState::states::ASCE):

            asce();
            clampZSP();

            break;

        case (AbstractLandState::states::R2LA):
            clampZSP();
            hold();
            break;

        case (AbstractLandState::states::COMP):
            hold();
            comp();
            break;
        case (AbstractLandState::states::LAND):
            land();
            break;

        default:
            hold();
            break;

    }
    _prevState = _actualState;

}

void Lander::resetSetPoint() {

    _setPoint = _state;
    _setPoint.setType(MavState::type::POSITION);

}

void Lander::updateIntegrals() {

    _err_int[0] +=  _dt * _err[0];
    _err_int[1] +=  _dt * _err[1];

    double tempx,tempy;

    tempx = common::clamp(_err_int[0],params_automatic::minIntValue,params_automatic::maxIntValue);
    tempy = common::clamp(_err_int[1],params_automatic::minIntValue,params_automatic::maxIntValue);

    _err_int[0] =  tempx;
    _err_int[1] =  tempy;

}

void Lander::managetime() {

    _actualTime = common::time::getTimeMilliSecond();
    double dt = _prevTime != 0 ? (_actualTime - _prevTime) * (double)MILLI2SECS : 0.0;
    _prevTime = _actualTime;
    _dt = dt;

}

void Lander::resetIntegrals() {

    _err_int[0] = 0;
    _err_int[1] = 0;

}

void Lander::init() {

    //Set point to my position
    resetSetPoint();

    //Go to max tracking height
    _setPoint.setZ(params_automatic::zMax);

}

void Lander::hold(bool increaseK) {

    //This function is purely tracking, nothing more

    /*
     * The tracking is performed by POSITION control, issuing the position setpoint
     * in order to achieve the desired velocity calculated by:
     *
     * Vdes = K * ep + Vplat
     * or
     * Psp = Pplat + K * Vplat
     *
     * where:
     *
     * Vdes = desired velocity, K = proportional gain, ep = position error, Vplat = paltform velocity
     */

    //Cache values
    MavState platPos = _platformState;
    MavState state   = _state;

    Eigen::Vector2d tempVel(platPos.getVx(),platPos.getVy());
    Eigen::Vector2d tempSetPoint(platPos.getX(),platPos.getY());
    //Eigen::Vector2d tempSetPoint(state.getX(),state.getY());
    Eigen::Vector2d tempErr = _err.head(2);

    //Increase proportional gain when needed
    double corr = 1;
    if(increaseK) corr *= 6;

    Eigen::Vector2d prop = corr * params_automatic::KpHold * tempErr;

    updateIntegrals();
    //PosSP = PlatPos + K * Vplat
    tempSetPoint += params_automatic::KpHoldV * tempVel + params_automatic::KiHold * _err_int.head(2);

    tempSetPoint += prop;

    //Fill right fields
    _setPoint.setPosition(tempSetPoint(0),tempSetPoint(1),_setPoint.getZ());
    _setPoint.setType(MavState::POSITION);

}

void Lander::asce() {

    resetIntegrals();
    _setPoint.setZ(_setPoint.getZ() + 0.1);

}

void Lander::desc() {

    //Add step because z is negative up
    _setPoint.setZ(_setPoint.getZ() - 0.1);
}

void Lander::comp() {

    MavState tempState = _state;
    //Calculate desired vertical velocity in order to compensate oscillations
    double dz = - _state.getZ() + _platformState.getZ() + PLATFORM_OFFSET;
    double desc = common::interpolate(fabs(dz), DRATE_MAX, DRATE_MIN, TMAX, TMIN);
    double z_target_v = _platformState.getVz() - desc;

    double err_v = z_target_v - tempState.getVz();

    double corr = 1;

    if(_platformState.getVz() > 0) corr = 1.2;

    z_target_v += corr * params_automatic::KPCompV * (err_v);

    //Now we need to transform this velocity in a position setpoint since in Firmware:
    // VelSP = Kp * PosError then PosSP = ( VelSP / Kp ) + RobotPos

    _setPoint.setZ((z_target_v) + tempState.getZ());

}

void Lander::clampZSP() {

    double temp;
    temp = common::clamp(_setPoint.getZ(),params_automatic::zMin,params_automatic::zMax);

    _setPoint.setZ(temp);
}

void Lander::land() {

    resetSetPoint();
    _setPoint.setZ(_state.getZ()-10);

}

<commit_msg>fix description<commit_after>//
// Created by andreanistico on 06/02/17.
//

#include <iostream>
#include "include/Lander/StatesClasses.hpp"
#include "Lander/Lander.h"
#include "parameters.h"

Lander::Lander()
        : _horizontaErr((double)0)    , _tauHold((double)0), _tauLost((double)0), _tauErr((double)0), _NHold(0),
          _NLost(0),_NComp(0), _initS(&_machine), _holdS(&_machine)  , _asceS(&_machine)  , _descS(&_machine),_compS(&_machine),
          _rtolS(&_machine),_landS(&_machine), _err(0,0,0), _err_int(0,0,0), _err_diff(0,0,0), _dt(0), _prevTime(0), _actualTime(0),_actualState(0),_prevState(0),
          _verticalErr(0)
{

    initStateMachine();
    _actualState = _machine.getActualNodeId();
    _err_prev = _err;

}

void Lander::setState(MavState pose) {
    _state = pose;
}

MavState Lander::getState() {
    return _state;
}

MavState Lander::getCommand() {
    return _setPoint;
}

void Lander::initStateMachine() {

    //Link signals
    _machine._horizontaErr =  &_horizontaErr;
    _machine._verticalErr  =  &_verticalErr;
    _machine._tauErr       =  &_tauErr;
    _machine._tauHold      =  &_tauHold;
    _machine._tauLost      =  &_tauLost;
    _machine._NLost        =  &_NLost;
    _machine._NHold        =  &_NHold;
    _machine._NComp        =  &_NComp;
    _machine._state        =  &_state;
    _machine._setPoint     =  &_setPoint;

    //Link states
    _initS._nextState    = &_holdS;
    _holdS._nextAscState = &_asceS;
    _holdS._nextDesState = &_descS;
    _holdS._nextState    = &_rtolS;
    _asceS._nextState    = &_holdS;
    _descS._nextState    = &_holdS;
    _rtolS._nextComState = &_compS;
    _rtolS._nextState    = &_holdS;
    _compS._nextState    = &_asceS;
    _compS._nextLanState = &_landS;
    _landS._nextState    = &_asceS;

    _machine.setStatePtr(&_initS);

    _tauHold = 0.5 * params_automatic::platformLenght;
    _tauLost = params_automatic::platformLenght * 0.7;

    //Print actual state
    std::cout << "Actual state: " << _machine.getActualNodeId() << std::endl;

}

void Lander::setPlatformState(const MavState platformState) {
    _platformState = platformState;
}

void Lander::updateSignals() {

    //Compute horizontal error
    double xTemp = _state.getX();
    double yTemp = _state.getY();
    double zTemp = _state.getZ();
    double xPlatTemp = _platformState.getX();
    double yPlatTemp = _platformState.getY();
    double zPlatTemp = _platformState.getZ();

    double dx = xPlatTemp - xTemp;
    double dy = yPlatTemp - yTemp;
    double dz = zPlatTemp - zTemp + PLATFORM_OFFSET;

    _err[0] = dx;
    _err[1] = dy;
    _err[2] = 0; //Trick, used to calculate horizontal error

    _horizontaErr = _err.norm();
    _err[2]       = dz;
    _verticalErr  = _err[2];

    if( _actualState == AbstractLandState::states::HOLD ||
        _actualState == AbstractLandState::states::ASCE ||
        _actualState == AbstractLandState::states::DESC ||
        _actualState == AbstractLandState::states::COMP ||
        _actualState == AbstractLandState::states::R2LA ){

        //Increment N if needed
        if (_horizontaErr < _tauHold) {
            _NLost = 0;
            _NHold++;
        }
        else if (_horizontaErr > _tauLost) {
            _NHold = 0;
            _NComp = 0;
            _NLost++;
        }

        if(_actualState == AbstractLandState::states::R2LA || _actualState == AbstractLandState::states::COMP){

            if(_prevState == AbstractLandState::states::HOLD) _NComp = 0;
            //Check whether we are on place to land
            if (_horizontaErr < _tauHold * 0.5) {
                _NComp++;
            } else{
                _NComp = 0;
            }
        }

        _err_prev = _err;
    }

#ifdef DEBUG

    std::cout << "**********************************" << std::endl;
    std::cout << "STATE: " << _actualState<< std::endl;
    std::cout << "HERRO: " << _horizontaErr<< std::endl;
    std::cout << "HERRV: " << _verticalErr<< std::endl;
    std::cout << "NHOLD: " << _NHold<< std::endl;
    std::cout << "NLOST: " << _NLost<< std::endl;
    std::cout << "NCOMP: " << _NComp<< std::endl;
    std::cout << "INTEX: " << _err_int[0] << std::endl;
    std::cout << "INTEY: " << _err_int[1] << std::endl;
    std::cout << "**********************************" << std::endl;
#endif

}

void Lander::handleMachine() {

    updateSignals();
    _machine.handle();
}

int Lander::getActualMachineState() {
    return _machine.getActualNodeId();
}

void Lander::run() {

    handleMachine();
    _actualState = _machine.getActualNodeId();
    managetime();

    static bool initDone = false;
    switch (_actualState){

        case (AbstractLandState::states::INIT):
            if(!initDone){
                init();
                initDone = true;
            }
            break;

        case (AbstractLandState::states::HOLD):
            initDone = false;
            clampZSP();
            hold();
            break;
        case (AbstractLandState::states::DESC):

            desc();
            clampZSP();
            break;
        case (AbstractLandState::states::ASCE):

            asce();
            clampZSP();

            break;

        case (AbstractLandState::states::R2LA):
            clampZSP();
            hold();
            break;

        case (AbstractLandState::states::COMP):
            hold();
            comp();
            break;
        case (AbstractLandState::states::LAND):
            land();
            break;

        default:
            hold();
            break;

    }
    _prevState = _actualState;

}

void Lander::resetSetPoint() {

    _setPoint = _state;
    _setPoint.setType(MavState::type::POSITION);

}

void Lander::updateIntegrals() {

    _err_int[0] +=  _dt * _err[0];
    _err_int[1] +=  _dt * _err[1];

    double tempx,tempy;

    tempx = common::clamp(_err_int[0],params_automatic::minIntValue,params_automatic::maxIntValue);
    tempy = common::clamp(_err_int[1],params_automatic::minIntValue,params_automatic::maxIntValue);

    _err_int[0] =  tempx;
    _err_int[1] =  tempy;

}

void Lander::managetime() {

    _actualTime = common::time::getTimeMilliSecond();
    double dt = _prevTime != 0 ? (_actualTime - _prevTime) * (double)MILLI2SECS : 0.0;
    _prevTime = _actualTime;
    _dt = dt;

}

void Lander::resetIntegrals() {

    _err_int[0] = 0;
    _err_int[1] = 0;

}

void Lander::init() {

    //Set point to my position
    resetSetPoint();

    //Go to max tracking height
    _setPoint.setZ(params_automatic::zMax);

}

void Lander::hold(bool increaseK) {

    //This function is purely tracking, nothing more

    /*
     * Tracking is performed by POSITION control, issuing the position setpoint
     * in order to achieve the desired velocity calculated by:
     *
     * Vdes = K * ep + Vplat
     * or
     * Psp = Pplat + K * Vplat
     *
     * where:
     *
     * Vdes = desired velocity, K = proportional gain, ep = position error, Vplat = paltform velocity
     */

    //Cache values
    MavState platPos = _platformState;
    MavState state   = _state;

    Eigen::Vector2d tempVel(platPos.getVx(),platPos.getVy());
    Eigen::Vector2d tempSetPoint(platPos.getX(),platPos.getY());
    //Eigen::Vector2d tempSetPoint(state.getX(),state.getY());
    Eigen::Vector2d tempErr = _err.head(2);

    //Increase proportional gain when needed
    double corr = 1;
    if(increaseK) corr *= 6;

    Eigen::Vector2d prop = corr * params_automatic::KpHold * tempErr;

    updateIntegrals();
    //PosSP = PlatPos + K * Vplat
    tempSetPoint += params_automatic::KpHoldV * tempVel + params_automatic::KiHold * _err_int.head(2);

    tempSetPoint += prop;

    //Fill right fields
    _setPoint.setPosition(tempSetPoint(0),tempSetPoint(1),_setPoint.getZ());
    _setPoint.setType(MavState::POSITION);

}

void Lander::asce() {

    resetIntegrals();
    _setPoint.setZ(_setPoint.getZ() + 0.1);

}

void Lander::desc() {

    //Add step because z is negative up
    _setPoint.setZ(_setPoint.getZ() - 0.1);
}

void Lander::comp() {

    MavState tempState = _state;
    //Calculate desired vertical velocity in order to compensate oscillations
    double dz = - _state.getZ() + _platformState.getZ() + PLATFORM_OFFSET;
    double desc = common::interpolate(fabs(dz), DRATE_MAX, DRATE_MIN, TMAX, TMIN);
    double z_target_v = _platformState.getVz() - desc;

    double err_v = z_target_v - tempState.getVz();

    double corr = 1;

    if(_platformState.getVz() > 0) corr = 1.2;

    z_target_v += corr * params_automatic::KPCompV * (err_v);

    //Now we need to transform this velocity in a position setpoint since in Firmware:
    // VelSP = Kp * PosError then PosSP = ( VelSP / Kp ) + RobotPos

    _setPoint.setZ((z_target_v) + tempState.getZ());

}

void Lander::clampZSP() {

    double temp;
    temp = common::clamp(_setPoint.getZ(),params_automatic::zMin,params_automatic::zMax);

    _setPoint.setZ(temp);
}

void Lander::land() {

    resetSetPoint();
    _setPoint.setZ(_state.getZ()-10);

}

<|endoftext|>


<commit_before>#ifdef HAVE_CMAKE_CONFIG
  #include "cmake_config.h"
#elif defined (HAVE_CONFIG_H)
  #include "config.h"
#endif // ifdef HAVE_CMAKE_CONFIG

#include <boost/filesystem.hpp>

#include <dune/common/exceptions.hh>
#include <dune/common/mpihelper.hh>
#include <dune/common/timer.hh>

#include <dune/stuff/common/parameter/tree.hh>
#include <dune/stuff/common/logging.hh>
#include <dune/stuff/grid/provider.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/function/expression.hh>

#include <dune/detailed/solvers/stationary/linear/elliptic/model.hh>
#include <dune/detailed/solvers/stationary/linear/elliptic/finitevolume/dune-pdelab.hh>

#ifdef POLORDER
  const int polOrder = POLORDER;
#else
  const int polOrder = 1;
#endif

const std::string id = "stationary.linear.elliptic.fv.dp";

using namespace Dune;
using namespace Dune::Detailed::Solvers;

/**
  \brief      Creates a parameter file if it does not exist.

              Nothing is done if the file already exists. If not, a parameter file will be created with all neccessary
              keys and values.
  \param[in]  filename
              (Relative) path to the file.
  **/
void ensureParamFile(std::string filename)
{
  // only write param file if there is none
  if (!boost::filesystem::exists(filename)) {
    std::ofstream file;
    file.open(filename);
    file << "[" << id << "]" << std::endl;
    file << "grid = stuff.grid.provider.cube" << std::endl;
    file << "model = detailed.solvers.stationary.linear.elliptic.model.default" << std::endl;
    file << "filename = " << id << std::endl;
    file << "[stuff.grid.provider.cube]" << std::endl;
    file << "lowerLeft = [0.0; 0.0; 0.0]" << std::endl;
    file << "upperRight = [1.0; 1.0; 1.0]" << std::endl;
    file << "numElements = 4" << std::endl;
    file << "[stuff.boundaryinfo.idbased]" << std::endl;
    file << "dirichlet = [1; 2; 3]" << std::endl;
    file << "neumann = [4]" << std::endl;
    file << "[detailed.solvers.stationary.linear.elliptic.model.default]" << std::endl;
    file << "diffusion.order = 0"  << std::endl;
    file << "diffusion.variable = x" << std::endl;
    file << "diffusion.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "force.order = 0"  << std::endl;
    file << "force.variable = x" << std::endl;
    file << "force.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "dirichlet.order = 0"  << std::endl;
    file << "dirichlet.variable = x" << std::endl;
    file << "dirichlet.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "neumann.order = 0"  << std::endl;
    file << "neumann.variable = x" << std::endl;
    file << "neumann.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "[detailed.solvers.stationary.linear.elliptic.model.thermalblock]" << std::endl;
    file << "diffusion.order = 0"  << std::endl;
    file << "diffusion.lowerLeft = [0.0; 0.0; 0.0]" << std::endl; // should coincide with the grid
    file << "diffusion.upperRight = [1.0; 1.0; 1.0]" << std::endl; // should coincide with the grid
    file << "diffusion.numElements = [2; 2; 2]"  << std::endl;
    file << "diffusion.components = [1.0; 10.0; 3.0; 2.1]"  << std::endl;
    file << "force.order = 0"  << std::endl;
    file << "force.variable = x" << std::endl;
    file << "force.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "dirichlet.order = 0"  << std::endl;
    file << "dirichlet.variable = x" << std::endl;
    file << "dirichlet.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "neumann.order = 0"  << std::endl;
    file << "neumann.variable = x" << std::endl;
    file << "neumann.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "[detailed.solvers.stationary.linear.elliptic.finitevolume.dune-pdelab]" << std::endl;
    file << "solve.type = eigen.bicgstab.incompletelut" << std::endl;
    file << "solve.maxIter = 5000"  << std::endl;
    file << "solve.precision = 1e-12"  << std::endl;
    file << "visualize.name = solution"  << std::endl;
    file.close();
  } // only write param file if there is none
} // void ensureParamFile()

int main(int argc, char** argv)
{
  try {
    // mpi
    MPIHelper::instance(argc, argv);

    // parameter
    const std::string filename = id + ".param";
    ensureParamFile(filename);
    Stuff::Common::ParameterTreeX paramTree(argc, argv, filename);
    paramTree.assertSub(id);

    // logger
    Stuff::Common::Logger().create(Stuff::Common::LOG_INFO |
                                   Stuff::Common::LOG_CONSOLE |
                                   Stuff::Common::LOG_DEBUG);
    Stuff::Common::LogStream& info = Stuff::Common::Logger().info();
    Stuff::Common::LogStream& debug = Stuff::Common::Logger().debug();

    // timer
    Timer timer;

    // grid
    info << "setting up grid: " << std::endl;
    debug.suspend();
    typedef Stuff::Grid::Provider::Interface<> GridProviderType;
    const std::string gridType = paramTree.get< std::string >(id + ".grid");
    const GridProviderType* gridProvider = Stuff::Grid::Provider::create(gridType,
                                                                         paramTree);
    typedef GridProviderType::GridType GridType;
    const shared_ptr< const GridType > grid = gridProvider->grid();
    typedef GridType::LeafGridView GridViewType;
    const shared_ptr< const GridViewType > gridView(new GridViewType(grid->leafView()));
    info << "  done (took " << timer.elapsed()
         << " sec, has " << grid->size(0) << " element";
    if (grid->size(0) > 1)
      info << "s";
    info << ")" << std::endl;
    info << "visualizing grid... " << std::flush;
    timer.reset();
    gridProvider->visualize(paramTree.get(id + ".filename", id) + ".grid");
    info << " done (took " << timer.elapsed() << " sek)" << std::endl;
    debug.resume();

    // model
    info << "setting up model... " << std::flush;
    timer.reset();
    const unsigned int DUNE_UNUSED(dimDomain) = GridProviderType::dim;
    const unsigned int DUNE_UNUSED(dimRange) = 1;
    typedef GridProviderType::CoordinateType::value_type DomainFieldType;
    typedef DomainFieldType RangeFieldType;
    typedef Stationary::Linear::Elliptic::Model::Interface< DomainFieldType, dimDomain,
                                                            RangeFieldType, dimRange >
        ModelType;
    const std::string modelType = paramTree.get< std::string >(id + ".model");
    const shared_ptr< const ModelType > model(
          Stationary::Linear::Elliptic::Model::create< DomainFieldType, dimDomain, RangeFieldType, dimRange >(modelType,
                                                                                paramTree));
    typedef Stuff::Grid::BoundaryInfo::IdBased BoundaryInfoType;
    const shared_ptr< const BoundaryInfoType > boundaryInfo(new BoundaryInfoType(
          BoundaryInfoType::createFromParamTree(paramTree)));
    info << "done (took " << timer.elapsed() << " sec)" << std::endl;

    // solver
    info << "initializing solver:" << std::endl;
    typedef Stationary::Linear::Elliptic::FiniteVolume::DunePdelab< ModelType,
                                                                    GridViewType,
                                                                    BoundaryInfoType,
                                                                    polOrder >
        SolverType;
    const Stuff::Common::ParameterTreeX solverTree = paramTree.sub(SolverType::id());
    SolverType solver(model, gridView, boundaryInfo);
    solver.init("  ", debug);

    info << "solving:" << std::endl;
    typedef SolverType::TrialVectorType SolutionType;
    Dune::shared_ptr< SolutionType > solution = solver.createVector();
    solver.solve(*solution, paramTree, "  ", debug);

    info << "postprocessing:" << std::endl;
    solver.visualize(*solution,
                     paramTree.get(id + ".filename", id) + ".solution",
                     solverTree.get("visualize.name", "solution"),
                     "  ",
                     debug);

    // if we came that far we can as well be happy about it
    return 0;
  } catch (Dune::Exception& e) {
    std::cerr << "Dune reported error: " << e.what() << std::endl;
  } catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
  } catch ( ... ) {
    std::cerr << "Unknown exception thrown!" << std::endl;
  } // try
} // main
<commit_msg>[...detailed.solvers...finitevolume.dune-pdelab.cc] renamed VectorType<commit_after>#ifdef HAVE_CMAKE_CONFIG
  #include "cmake_config.h"
#elif defined (HAVE_CONFIG_H)
  #include "config.h"
#endif // ifdef HAVE_CMAKE_CONFIG

#include <boost/filesystem.hpp>

#include <dune/common/exceptions.hh>
#include <dune/common/mpihelper.hh>
#include <dune/common/timer.hh>

#include <dune/stuff/common/parameter/tree.hh>
#include <dune/stuff/common/logging.hh>
#include <dune/stuff/grid/provider.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/function/expression.hh>

#include <dune/detailed/solvers/stationary/linear/elliptic/model.hh>
#include <dune/detailed/solvers/stationary/linear/elliptic/finitevolume/dune-pdelab.hh>

#ifdef POLORDER
  const int polOrder = POLORDER;
#else
  const int polOrder = 1;
#endif

const std::string id = "stationary.linear.elliptic.fv.dp";

using namespace Dune;
using namespace Dune::Detailed::Solvers;

/**
  \brief      Creates a parameter file if it does not exist.

              Nothing is done if the file already exists. If not, a parameter file will be created with all neccessary
              keys and values.
  \param[in]  filename
              (Relative) path to the file.
  **/
void ensureParamFile(std::string filename)
{
  // only write param file if there is none
  if (!boost::filesystem::exists(filename)) {
    std::ofstream file;
    file.open(filename);
    file << "[" << id << "]" << std::endl;
    file << "grid = stuff.grid.provider.cube" << std::endl;
    file << "model = detailed.solvers.stationary.linear.elliptic.model.default" << std::endl;
    file << "filename = " << id << std::endl;
    file << "[stuff.grid.provider.cube]" << std::endl;
    file << "lowerLeft = [0.0; 0.0; 0.0]" << std::endl;
    file << "upperRight = [1.0; 1.0; 1.0]" << std::endl;
    file << "numElements = 4" << std::endl;
    file << "[stuff.boundaryinfo.idbased]" << std::endl;
    file << "dirichlet = [1; 2; 3]" << std::endl;
    file << "neumann = [4]" << std::endl;
    file << "[detailed.solvers.stationary.linear.elliptic.model.default]" << std::endl;
    file << "diffusion.order = 0"  << std::endl;
    file << "diffusion.variable = x" << std::endl;
    file << "diffusion.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "force.order = 0"  << std::endl;
    file << "force.variable = x" << std::endl;
    file << "force.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "dirichlet.order = 0"  << std::endl;
    file << "dirichlet.variable = x" << std::endl;
    file << "dirichlet.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "neumann.order = 0"  << std::endl;
    file << "neumann.variable = x" << std::endl;
    file << "neumann.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "[detailed.solvers.stationary.linear.elliptic.model.thermalblock]" << std::endl;
    file << "diffusion.order = 0"  << std::endl;
    file << "diffusion.lowerLeft = [0.0; 0.0; 0.0]" << std::endl; // should coincide with the grid
    file << "diffusion.upperRight = [1.0; 1.0; 1.0]" << std::endl; // should coincide with the grid
    file << "diffusion.numElements = [2; 2; 2]"  << std::endl;
    file << "diffusion.components = [1.0; 10.0; 3.0; 2.1]"  << std::endl;
    file << "force.order = 0"  << std::endl;
    file << "force.variable = x" << std::endl;
    file << "force.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "dirichlet.order = 0"  << std::endl;
    file << "dirichlet.variable = x" << std::endl;
    file << "dirichlet.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "neumann.order = 0"  << std::endl;
    file << "neumann.variable = x" << std::endl;
    file << "neumann.expression = [1.0; 1.0; 1.0]"  << std::endl;
    file << "[detailed.solvers.stationary.linear.elliptic.finitevolume.dune-pdelab]" << std::endl;
    file << "solve.type = eigen.bicgstab.incompletelut" << std::endl;
    file << "solve.maxIter = 5000"  << std::endl;
    file << "solve.precision = 1e-12"  << std::endl;
    file << "visualize.name = solution"  << std::endl;
    file.close();
  } // only write param file if there is none
} // void ensureParamFile()

int main(int argc, char** argv)
{
  try {
    // mpi
    MPIHelper::instance(argc, argv);

    // parameter
    const std::string filename = id + ".param";
    ensureParamFile(filename);
    Stuff::Common::ParameterTreeX paramTree(argc, argv, filename);
    paramTree.assertSub(id);

    // logger
    Stuff::Common::Logger().create(Stuff::Common::LOG_INFO |
                                   Stuff::Common::LOG_CONSOLE |
                                   Stuff::Common::LOG_DEBUG);
    Stuff::Common::LogStream& info = Stuff::Common::Logger().info();
    Stuff::Common::LogStream& debug = Stuff::Common::Logger().debug();

    // timer
    Timer timer;

    // grid
    info << "setting up grid: " << std::endl;
    debug.suspend();
    typedef Stuff::Grid::Provider::Interface<> GridProviderType;
    const std::string gridType = paramTree.get< std::string >(id + ".grid");
    const GridProviderType* gridProvider = Stuff::Grid::Provider::create(gridType,
                                                                         paramTree);
    typedef GridProviderType::GridType GridType;
    const shared_ptr< const GridType > grid = gridProvider->grid();
    typedef GridType::LeafGridView GridViewType;
    const shared_ptr< const GridViewType > gridView(new GridViewType(grid->leafView()));
    info << "  done (took " << timer.elapsed()
         << " sec, has " << grid->size(0) << " element";
    if (grid->size(0) > 1)
      info << "s";
    info << ")" << std::endl;
    info << "visualizing grid... " << std::flush;
    timer.reset();
    gridProvider->visualize(paramTree.get(id + ".filename", id) + ".grid");
    info << " done (took " << timer.elapsed() << " sek)" << std::endl;
    debug.resume();

    // model
    info << "setting up model... " << std::flush;
    timer.reset();
    const unsigned int DUNE_UNUSED(dimDomain) = GridProviderType::dim;
    const unsigned int DUNE_UNUSED(dimRange) = 1;
    typedef GridProviderType::CoordinateType::value_type DomainFieldType;
    typedef DomainFieldType RangeFieldType;
    typedef Stationary::Linear::Elliptic::Model::Interface< DomainFieldType, dimDomain,
                                                            RangeFieldType, dimRange >
        ModelType;
    const std::string modelType = paramTree.get< std::string >(id + ".model");
    const shared_ptr< const ModelType > model(
          Stationary::Linear::Elliptic::Model::create< DomainFieldType, dimDomain, RangeFieldType, dimRange >(modelType,
                                                                                paramTree));
    typedef Stuff::Grid::BoundaryInfo::IdBased BoundaryInfoType;
    const shared_ptr< const BoundaryInfoType > boundaryInfo(new BoundaryInfoType(
          BoundaryInfoType::createFromParamTree(paramTree)));
    info << "done (took " << timer.elapsed() << " sec)" << std::endl;

    // solver
    info << "initializing solver:" << std::endl;
    typedef Stationary::Linear::Elliptic::FiniteVolume::DunePdelab< ModelType,
                                                                    GridViewType,
                                                                    BoundaryInfoType,
                                                                    polOrder >
        SolverType;
    const Stuff::Common::ParameterTreeX solverTree = paramTree.sub(SolverType::id());
    SolverType solver(model, gridView, boundaryInfo);
    solver.init("  ", debug);

    info << "solving:" << std::endl;
    typedef SolverType::VectorBackendType SolutionType;
    Dune::shared_ptr< SolutionType > solution = solver.createVector();
    solver.solve(*solution, paramTree, "  ", debug);

    info << "postprocessing:" << std::endl;
    solver.visualize(*solution,
                     paramTree.get(id + ".filename", id) + ".solution",
                     solverTree.get("visualize.name", "solution"),
                     "  ",
                     debug);

    // if we came that far we can as well be happy about it
    return 0;
  } catch (Dune::Exception& e) {
    std::cerr << "Dune reported error: " << e.what() << std::endl;
  } catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
  } catch ( ... ) {
    std::cerr << "Unknown exception thrown!" << std::endl;
  } // try
} // main
<|endoftext|>


<commit_before>/*
 * The Apache Software License, Version 1.1
 *
 *
 * Copyright (c) 1999 The Apache Software Foundation.  All rights
 * reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xalan" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com.  For more
 * information on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 *
 * XMLFileReporter.hpp
 */
#if !defined(HARNESS_HEADER_GUARD_1357924680)
#define HARNESS_HEADER_GUARD_1357924680

#include<stdio.h>
#include <time.h>

//#include <util/PlatformUtils.hpp>
#include <PlatformSupport/DOMStringHelper.hpp>

#include<map>

/**
 * Reporter that saves output to a simple XML-format file.
 * @author Shane_Curcuru@lotus.com
 * @version $Id$
 */

#if defined HARNESS_EXPORTS
#define HARNESS_API __declspec(dllexport)
#else
#define HARNESS_API __declspec(dllimport)
#endif

// This class is exported from the Harness.dll
class HARNESS_API XMLFileReporter
{

public:

#if defined(XALAN_NO_NAMESPACES)
	typedef map<XalanDOMString, XalanDOMString, less<XalanDOMString> >	Hashtable;
#else
	typedef std::map<XalanDOMString, XalanDOMString>	Hashtable;
#endif

	/** Simple constructor, does not perform initialization.  */
	XMLFileReporter();

	/** Construct and initialize this reporter with specified filename.  */
	XMLFileReporter(const XalanDOMString& fileName);

	/** Construct and initialize this reporter with specified filename.  */
	XMLFileReporter::XMLFileReporter(const char* fileName);

	/** Initialize this XMLFileReporter.  Must be called before attempting to log anything.  */
	bool initialize();

	/** Accessor for flushing; is set from properties.  */
	bool getFlushOnCaseClose();

	/** Accessor methods for our properties block.  */
	const XalanDOMString& getFileName() const;

	/** Accessor methods for our properties block.  */
	void setFileName(const XalanDOMString& fileName);

	/** Accessor methods for our properties block.  */
	void setFileName(const char* fileName)
	{
		setFileName(XalanDOMString(fileName));
	}

	/**
	* Is this Reporter still running OK?
	* @returns status - true if an error has occoured, false if it's still working fine
	*
XMLFileReporter should just about never have errors.

	*/
	bool checkError();

	/**
	* Is this Reporter ready to log results?
	* @returns status - true if it's ready to report, false otherwise
	*/
	bool isReady();

	/** Flush this reporter - ensure our File is flushed.  */
	void flush();

	/** Close this reporter - ensure our File, etc. are closed.  */
	void close();

	//-----------------------------------------------------
	//-------- Testfile / Testcase start and stop routines --------
	//-----------------------------------------------------

	/**
	* Report that a testfile has started.
	* @param msg message to log out
	*/
	void logTestFileInit(const XalanDOMString& msg);

	void logTestFileInit(const char*	msg)
	{
		logTestFileInit(XalanDOMString(msg));
	}

	/**
	* Report that a testfile has finished, and report it's result.
	* @param msg message to log out
	* @param result result of testfile
	*/
	void logTestFileClose(const XalanDOMString& msg, const XalanDOMString& result);

	void logTestFileClose(const char* msg, const char* result)
	{
		logTestFileClose(XalanDOMString(msg), XalanDOMString(result));
	}

	void logTestCaseInit(const XalanDOMString& msg);

	void logTestCaseInit(const char*	msg)
	{
		logTestCaseInit(XalanDOMString(msg));
	}

	/**
	* Report that a testcase has finished, and report it's result.
	* @param msg message to log out
	* @param result result of testfile
	*/
	void logTestCaseClose(const XalanDOMString& msg, const XalanDOMString& result);

	void logTestCaseClose(const char* msg, const char* result)
	{
		logTestCaseClose(XalanDOMString(msg), XalanDOMString(result));
	}
	//-----------------------------------------------------
	//-------- Test results reporting and logging routines --------
	//-----------------------------------------------------

	/**
	* Report a comment to result file with specified severity.
	*
Record format: <message level="##">msg</message>

	* @param level severity or class of message.
	* @param msg comment to log out.
	*/
	void logMessage(int level, const XalanDOMString& msg);

	/**
	* Logs out statistics to result file with specified severity.
	*
Record format: <statistic level="##" desc="msg"><longval>1234</longval><doubleval>1.234</doubleval></statistic>

	* @param level severity of message.
	* @param lVal statistic in long format.
	* @param dVal statistic in double format.
	* @param msg comment to log out.
	*/
	void logStatistic (int level, long lVal, double dVal, const XalanDOMString& msg);

	void logStatistic (int level, long lVal, double dVal, const char*	msg)
	{
		logStatistic(level, lVal, dVal, XalanDOMString(msg));
	}

	/**
    * Logs out a element to results with specified severity.
    * Uses user-supplied element name and attribute list.  Currently
    * attribute values and msg are forced .toString().  Also,
    * 'level' is forced to be the first attribute of the element.
    * @param level severity of message.
    * @param element name of enclosing element
    * @param attrs hash of name=value attributes; note that the
    * caller must ensure they're legal XML
	* @param msg comment to log out.
	*/
	void logElement(int level, const XalanDOMString& element, Hashtable attrs, const XalanDOMString& msg);

	void logElement(int level,  const char* element, Hashtable attrs, const char* msg)
	{
		logElement(level, XalanDOMString(element), attrs, XalanDOMString(msg));
	}

	/**
	* Report an arbitrary XalanDOMString to result file with specified severity.
	*
Appends and prepends \\n newline characters at the start and
	* end of the message to separate it from the tags.

	*
Record format: <arbitrary level="##">

	* msg

	* </arbitrary>
	*

	* @param level severity or class of message.
	* @param msg arbitrary XalanDOMString to log out.
	*/
	void logArbitraryMessage (int level, const XalanDOMString& msg);

	/**
	* Report a complete Hashtable to result file with specified severity.
	*
Indents each hashitem within the table.

	*
Record format: <hashtable level="##" desc="msg"/>

	*   <hashitem key="key1">value1</hashitem>

	*   <hashitem key="key2">value2</hashitem>

	* </hashtable>
	*

	* @param level severity or class of message.
	* @param hash Hashtable to log the contents of.
	* @param msg decription of the Hashtable.
	*/
	/*
	void logHashtable (int level, Hashtable hash, const XalanDOMString& msg);
	*/

	/**
	* Writes out a Pass record with comment.
	*
Record format: <checkresult result="PASS" desc="comment"/>

	* @param comment comment to log with the pass record.
	*/
	void logCheckPass(const XalanDOMString& comment);

	/**
	* Writes out an ambiguous record with comment.
	*
Record format: <checkresult result="AMBG" desc="comment"/>

	* @param comment comment to log with the ambg record.
	*/
	void logCheckAmbiguous(const XalanDOMString& comment);

	/**
	* Writes out a Fail record with comment.
	*
Record format: <checkresult result="FAIL" desc="comment"/>

	* @param comment comment to log with the fail record.
	*/
	void logCheckFail(const XalanDOMString& comment);

	/**
	* Writes out a Error record with comment.
	*
Record format: <checkresult result="ERRR" desc="comment"/>

	* @param comment comment to log with the error record.
	*/
	void logCheckErr(const XalanDOMString& comment);

	/**
	* Escapes a XalanDOMString to remove < and > so it's valid XML.
	*
Stolen mostly from Xalan applet sample.

	* @param s XalanDOMString to escape.
	* @return XalanDOMString that has been escaped.
	*/
	XalanDOMString escapestring(const XalanDOMString& s);

private:

	/**
	* worker method to dump the xml header and open the resultsfile element.
	*/
	bool startResultsFile();

	/**
	* worker method to close the resultsfile element.
	*/
	bool closeResultsFile();

	/**
	* worker method to prints to the resultsfile.
	*/
	bool printToFile(const XalanDOMString& output);

	/**
	* worker method to prints to the resultsfile.
	*/
	XalanDOMString getDateTimeString();

	/** Key for Properties block that denotes our output filename.  */
	static const XalanDOMString OPT_FILENAME;

	/** XML tagnames for results file structure.  */
	static const XalanDOMString ELEM_RESULTSFILE;
	static const XalanDOMString ELEM_TESTFILE;
	static const XalanDOMString ELEM_FILERESULT;
	static const XalanDOMString ELEM_TESTCASE;
	static const XalanDOMString ELEM_CASERESULT;
	static const XalanDOMString ELEM_CHECKRESULT;
	static const XalanDOMString ELEM_STATISTIC;
	static const XalanDOMString ELEM_LONGVAL;
	static const XalanDOMString ELEM_DOUBLEVAL;
	static const XalanDOMString ELEM_MESSAGE;
	static const XalanDOMString ELEM_ARBITRARY;
	static const XalanDOMString ELEM_HASHTABLE;
	static const XalanDOMString ELEM_HASHITEM;

	/** XML attribute names for results file structure.  */
	static const XalanDOMString ATTR_LEVEL;
	static const XalanDOMString ATTR_DESC;
	static const XalanDOMString ATTR_TIME;
	static const XalanDOMString ATTR_RESULT;
	static const XalanDOMString ATTR_KEY;
	static const XalanDOMString ATTR_FILENAME;

	/**
	* Optimization: for heavy use methods, form pre-defined constants to save on XalanDOMString concatenation.
	*
Note the indent; must be updated if we ever switch to another indenting method.

	*/
	static const XalanDOMString TESTCASEINIT_HDR;
	static const XalanDOMString TESTCASECLOSE_HDR;
	static const XalanDOMString MESSAGE_HDR;
	static const XalanDOMString STATISTIC_HDR;
	static const XalanDOMString ARBITRARY_HDR;
	static const XalanDOMString HASHTABLE_HDR;
	static const XalanDOMString HASHITEM_HDR;
	static const XalanDOMString CHECKPASS_HDR;
	static const XalanDOMString CHECKAMBG_HDR;
	static const XalanDOMString CHECKERRR_HDR;
	static const XalanDOMString CHECKFAIL_HDR;

	/** If we have output anything yet.  */
	bool m_anyOutput;

	/** Name of the file we're outputing to.  */
	XalanDOMString m_fileName;

	/** File reference and other internal convenience variables.  */
	FILE* m_fileHandle;

	/** If we're ready to start outputting yet.  */
	bool m_ready;

	/** If an error has occoured in this Reporter.  */
	bool m_error;

	/** If we should flush after every logTestCaseClose.  */
	bool m_flushOnCaseClose;

};        // end of class XMLFileReporter
#endif<commit_msg>Cleaned up comments<commit_after>/*
 * The Apache Software License, Version 1.1
 *
 *
 * Copyright (c) 1999 The Apache Software Foundation.  All rights
 * reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xalan" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com.  For more
 * information on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 *
 * XMLFileReporter.hpp
 */
#if !defined(HARNESS_HEADER_GUARD_1357924680)
#define HARNESS_HEADER_GUARD_1357924680

#include<stdio.h>
#include <time.h>

//#include <util/PlatformUtils.hpp>
#include <PlatformSupport/DOMStringHelper.hpp>

#include<map>

/**
 * Reporter that saves output to a simple XML-format file.
 * @author Shane_Curcuru@lotus.com
 * @version $Id$
 */

#if defined HARNESS_EXPORTS
#define HARNESS_API __declspec(dllexport)
#else
#define HARNESS_API __declspec(dllimport)
#endif

#if defined(XALAN_NO_NAMESPACES)
	typedef map<XalanDOMString, XalanDOMString, less<XalanDOMString> >	Hashtable;
#else
	typedef std::map<XalanDOMString, XalanDOMString>	Hashtable;
#endif

// This class is exported from the Harness.dll
class HARNESS_API XMLFileReporter
{

public:

	/** Simple constructor, does not perform initialization.  */
	XMLFileReporter();

	/** Construct and initialize this reporter with specified filename.  */
	XMLFileReporter(const XalanDOMString& fileName);

	/** Construct and initialize this reporter with specified filename.  */
	XMLFileReporter::XMLFileReporter(const char* fileName);

	/** Initialize this XMLFileReporter.  Must be called before attempting to log anything.  */
	bool initialize();

	/** Accessor for flushing; is set from properties.  */
	bool getFlushOnCaseClose();

	/** Accessor methods for our properties block.  */
	const XalanDOMString& getFileName() const;

	/** Accessor methods for our properties block.  */
	void setFileName(const XalanDOMString& fileName);

	/** Accessor methods for our properties block.  */
	void setFileName(const char* fileName)
	{
		setFileName(XalanDOMString(fileName));
	}

	/**
	* Is this Reporter still running OK?
	* @returns status - true if an error has occoured, false if it's still working fine
	*
XMLFileReporter should just about never have errors.

	*/
	bool checkError();

	/**
	* Is this Reporter ready to log results?
	* @returns status - true if it's ready to report, false otherwise
	*/
	bool isReady();

	/** Flush this reporter - ensure our File is flushed.  */
	void flush();

	/** Close this reporter - ensure our File, etc. are closed.  */
	void close();

	//-----------------------------------------------------
	//-------- Testfile / Testcase start and stop routines --------
	//-----------------------------------------------------

	/**
	* Report that a testfile has started.
	* @param msg message to log out
	*/
	void logTestFileInit(const XalanDOMString& msg);

	void logTestFileInit(const char*	msg)
	{
		logTestFileInit(XalanDOMString(msg));
	}

	/**
	* Report that a testfile has finished, and report it's result.
	* @param msg message to log out
	* @param result result of testfile
	*/
	void logTestFileClose(const XalanDOMString& msg, const XalanDOMString& result);

	void logTestFileClose(const char* msg, const char* result)
	{
		logTestFileClose(XalanDOMString(msg), XalanDOMString(result));
	}

	void logTestCaseInit(const XalanDOMString& msg);

	void logTestCaseInit(const char*	msg)
	{
		logTestCaseInit(XalanDOMString(msg));
	}

	/**
	* Report that a testcase has finished, and report it's result.
	* @param msg message to log out
	* @param result result of testfile
	*/
	void logTestCaseClose(const XalanDOMString& msg, const XalanDOMString& result);

	void logTestCaseClose(const char* msg, const char* result)
	{
		logTestCaseClose(XalanDOMString(msg), XalanDOMString(result));
	}
	//-----------------------------------------------------
	//-------- Test results reporting and logging routines --------
	//-----------------------------------------------------

	/**
	* Report a comment to result file with specified severity.
	*
Record format: <message level="##">msg</message>

	* @param level severity or class of message.
	* @param msg comment to log out.
	*/
	void logMessage(int level, const XalanDOMString& msg);

	/**
	* Logs out statistics to result file with specified severity.
	*
Record format: <statistic level="##" desc="msg"><longval>1234</longval><doubleval>1.234</doubleval></statistic>

	* @param level severity of message.
	* @param lVal statistic in long format.
	* @param dVal statistic in double format.
	* @param msg comment to log out.
	*/
	void logStatistic (int level, long lVal, double dVal, const XalanDOMString& msg);

	void logStatistic (int level, long lVal, double dVal, const char*	msg)
	{
		logStatistic(level, lVal, dVal, XalanDOMString(msg));
	}

	/**
    * Logs out a element to results with specified severity.
    * Uses user-supplied element name and attribute list.  Currently
    * attribute values and msg are forced .toString().  Also,
    * 'level' is forced to be the first attribute of the element.
    * @param level severity of message.
    * @param element name of enclosing element
    * @param attrs hash of name=value attributes; note that the
    * caller must ensure they're legal XML
	* @param msg comment to log out.
	*/
	void logElement(int level, const XalanDOMString& element, Hashtable attrs, const XalanDOMString& msg);

	void logElement(int level,  const char* element, Hashtable attrs, const char* msg)
	{
		logElement(level, XalanDOMString(element), attrs, XalanDOMString(msg));
	}

	/**
	* Report an arbitrary XalanDOMString to result file with specified severity.
	*
Appends and prepends \\n newline characters at the start and
	* end of the message to separate it from the tags.

	*
Record format: <arbitrary level="##">

	* msg

	* </arbitrary>
	*

	* @param level severity or class of message.
	* @param msg arbitrary XalanDOMString to log out.
	*/
	void logArbitraryMessage (int level, const XalanDOMString& msg);

	/**
	* Report a complete Hashtable to result file with specified severity.
	*
Indents each hashitem within the table.

	*
Record format: <hashtable level="##" desc="msg"/>

	*   <hashitem key="key1">value1</hashitem>

	*   <hashitem key="key2">value2</hashitem>

	* </hashtable>
	*

	* @param level severity or class of message.
	* @param hash Hashtable to log the contents of.
	* @param msg decription of the Hashtable.
	*/
	/*
	void logHashtable (int level, Hashtable hash, const XalanDOMString& msg);
	*/

	/**
	* Writes out a Pass record with comment.
	*
Record format: <checkresult result="PASS" desc="comment"/>

	* @param comment comment to log with the pass record.
	*/
	void logCheckPass(const XalanDOMString& comment);

	/**
	* Writes out an ambiguous record with comment.
	*
Record format: <checkresult result="AMBG" desc="comment"/>

	* @param comment comment to log with the ambg record.
	*/
	void logCheckAmbiguous(const XalanDOMString& comment);

	/**
	* Writes out a Fail record with comment.
	*
Record format: <checkresult result="FAIL" desc="comment"/>

	* @param comment comment to log with the fail record.
	*/
	void logCheckFail(const XalanDOMString& comment);

	/**
	* Writes out a Error record with comment.
	*
Record format: <checkresult result="ERRR" desc="comment"/>

	* @param comment comment to log with the error record.
	*/
	void logCheckErr(const XalanDOMString& comment);

	/**
	* Escapes a XalanDOMString to remove < and > so it's valid XML.
	*
Stolen mostly from Xalan applet sample.

	* @param s XalanDOMString to escape.
	* @return XalanDOMString that has been escaped.
	*/
	XalanDOMString escapestring(const XalanDOMString& s);

private:

	/**
	* worker method to dump the xml header and open the resultsfile element.
	*/
	bool startResultsFile();

	/**
	* worker method to close the resultsfile element.
	*/
	bool closeResultsFile();

	/**
	* worker method to prints to the resultsfile.
	*/
	bool printToFile(const XalanDOMString& output);

	/**
	* worker method to prints to the resultsfile.
	*/
	XalanDOMString getDateTimeString();

	/** Key for Properties block that denotes our output filename.  */
	static const XalanDOMString OPT_FILENAME;

	/** XML tagnames for results file structure.  */
	static const XalanDOMString ELEM_RESULTSFILE;
	static const XalanDOMString ELEM_TESTFILE;
	static const XalanDOMString ELEM_FILERESULT;
	static const XalanDOMString ELEM_TESTCASE;
	static const XalanDOMString ELEM_CASERESULT;
	static const XalanDOMString ELEM_CHECKRESULT;
	static const XalanDOMString ELEM_STATISTIC;
	static const XalanDOMString ELEM_LONGVAL;
	static const XalanDOMString ELEM_DOUBLEVAL;
	static const XalanDOMString ELEM_MESSAGE;
	static const XalanDOMString ELEM_ARBITRARY;
	static const XalanDOMString ELEM_HASHTABLE;
	static const XalanDOMString ELEM_HASHITEM;

	/** XML attribute names for results file structure.  */
	static const XalanDOMString ATTR_LEVEL;
	static const XalanDOMString ATTR_DESC;
	static const XalanDOMString ATTR_TIME;
	static const XalanDOMString ATTR_RESULT;
	static const XalanDOMString ATTR_KEY;
	static const XalanDOMString ATTR_FILENAME;

	/**
	* Optimization: for heavy use methods, form pre-defined constants to save on XalanDOMString concatenation.
	*
Note the indent; must be updated if we ever switch to another indenting method.

	*/
	static const XalanDOMString TESTCASEINIT_HDR;
	static const XalanDOMString TESTCASECLOSE_HDR;
	static const XalanDOMString MESSAGE_HDR;
	static const XalanDOMString STATISTIC_HDR;
	static const XalanDOMString ARBITRARY_HDR;
	static const XalanDOMString HASHTABLE_HDR;
	static const XalanDOMString HASHITEM_HDR;
	static const XalanDOMString CHECKPASS_HDR;
	static const XalanDOMString CHECKAMBG_HDR;
	static const XalanDOMString CHECKERRR_HDR;
	static const XalanDOMString CHECKFAIL_HDR;

	/** If we have output anything yet.  */
	bool m_anyOutput;

	/** Name of the file we're outputing to.  */
	XalanDOMString m_fileName;

	/** File reference and other internal convenience variables.  */
	FILE* m_fileHandle;

	/** If we're ready to start outputting yet.  */
	bool m_ready;

	/** If an error has occoured in this Reporter.  */
	bool m_error;

	/** If we should flush after every logTestCaseClose.  */
	bool m_flushOnCaseClose;

};        // end of class XMLFileReporter
#endif<|endoftext|>


<commit_before><commit_msg>Fixed problem with allowing attribute with unknown prefix.  Use provided prefix whenever possible.<commit_after><|endoftext|>


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

#include "mbed.h"
#include "us_ticker_api.h"
#include "BLE.h"
#include "wsf_types.h"
#include "wsf_msg.h"
#include "wsf_os.h"
#include "wsf_buf.h"
#include "wsf_timer.h"
#include "hci_handler.h"
#include "dm_handler.h"
#include "l2c_handler.h"
#include "att_handler.h"
#include "smp_handler.h"
#include "l2c_api.h"
#include "att_api.h"
#include "smp_api.h"
#include "hci_drv.h"
#include "CordioBLE.h"
#include "mbed_assert.h"

#include "CordioPalAttClient.h"
#include "CordioPalSecurityManager.h"

/*! WSF handler ID */
wsfHandlerId_t stack_handler_id;

/**
 * Weak definition of ble_cordio_get_hci_driver.
 * A runtime error is generated if the user does not define any
 * ble_cordio_get_hci_driver.
 */
MBED_WEAK ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver()
{
    error("Please provide an implementation for the HCI driver");
    ble::vendor::cordio::CordioHCIDriver* bad_instance = NULL;
    return *bad_instance;
}

/**
 * Low level HCI interface between Cordio stack and the port.
 */
extern "C" uint16_t hci_mbed_os_drv_write(uint8_t type, uint16_t len, uint8_t *pData)
{
    return ble_cordio_get_hci_driver().write(type, len, pData);
}

extern "C" void hci_mbed_os_start_reset_sequence(void)
{
    ble_cordio_get_hci_driver().start_reset_sequence();
}

extern "C" void hci_mbed_os_handle_reset_sequence(uint8_t* msg)
{
     ble_cordio_get_hci_driver().handle_reset_sequence(msg);
}

/*
 * This function will signal to the user code by calling signalEventsToProcess.
 * It is registered and called into the Wsf Stack.
 */
extern "C" void wsf_mbed_ble_signal_event(void)
{
    ble::vendor::cordio::BLE::deviceInstance().signalEventsToProcess(::BLE::DEFAULT_INSTANCE);
}

/**
 * BLE-API requires an implementation of the following function in order to
 * obtain its transport handle.
 */
BLEInstanceBase *createBLEInstance()
{
    return (&(ble::vendor::cordio::BLE::deviceInstance()));
}

namespace ble {
namespace vendor {
namespace cordio {

BLE::BLE(CordioHCIDriver& hci_driver) :
    initialization_status(NOT_INITIALIZED),
    instanceID(::BLE::DEFAULT_INSTANCE),
    _event_queue()
{
    _hci_driver = &hci_driver;
    stack_setup();
}

BLE::~BLE() { }

/**
 * The singleton which represents the BLE transport for the BLE.
 */
BLE& BLE::deviceInstance()
{
    static BLE instance(
        ble_cordio_get_hci_driver()
    );
    return instance;
}

ble_error_t BLE::init(
    ::BLE::InstanceID_t instanceID,
    FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext *> initCallback)
{
    switch (initialization_status) {
        case NOT_INITIALIZED:
            _event_queue.initialize(this, instanceID);
            _init_callback = initCallback;
            start_stack_reset();
            return BLE_ERROR_NONE;

        case INITIALIZING:
            return BLE_ERROR_INITIALIZATION_INCOMPLETE;

        case INITIALIZED:
            return BLE_ERROR_NONE;

        default:
            return BLE_ERROR_UNSPECIFIED;
    }
}

bool BLE::hasInitialized() const
{
    return initialization_status == INITIALIZED;
}

ble_error_t BLE::shutdown()
{
    if (initialization_status != INITIALIZED) {
        return BLE_ERROR_INITIALIZATION_INCOMPLETE;
    }

    initialization_status = NOT_INITIALIZED;
    _hci_driver->terminate();

    getGattServer().reset();
    getGattClient().reset();
    getGap().reset();
    _event_queue.clear();

    return BLE_ERROR_NONE;
}

const char* BLE::getVersion()
{
    static const char version[] = "generic-cordio";
    return version;
}

generic::GenericGap& BLE::getGap()
{
    static pal::vendor::cordio::GenericAccessService cordio_gap_service;
    static ble::generic::GenericGap gap(
        _event_queue,
        pal::vendor::cordio::Gap::get_gap(),
        cordio_gap_service,
        pal::vendor::cordio::CordioSecurityManager::get_security_manager()
    );

    return gap;
}

const generic::GenericGap& BLE::getGap() const
{
    BLE &self = const_cast<BLE&>(*this);
    return const_cast<const generic::GenericGap&>(self.getGap());
};

GattServer& BLE::getGattServer()
{
    return cordio::GattServer::getInstance();
}

const GattServer& BLE::getGattServer() const
{
    return cordio::GattServer::getInstance();
}

generic::GenericGattClient& BLE::getGattClient()
{
    static pal::AttClientToGattClientAdapter pal_client(
        pal::vendor::cordio::CordioAttClient::get_client()
    );
    static generic::GenericGattClient client(&pal_client);

    return client;
}

SecurityManager& BLE::getSecurityManager()
{
    static SigningEventMonitorProxy signing_event_monitor(*this);
    static generic::GenericSecurityManager m_instance(
        pal::vendor::cordio::CordioSecurityManager::get_security_manager(),
        getGap(),
        signing_event_monitor
    );

    return m_instance;
}

const SecurityManager& BLE::getSecurityManager() const
{
    const BLE &self = const_cast<BLE&>(*this);
    return const_cast<const SecurityManager&>(self.getSecurityManager());
}

void BLE::waitForEvent()
{
    static Timeout nextTimeout;
    timestamp_t nextTimestamp;
    bool_t pTimerRunning;

    callDispatcher();

    if (wsfOsReadyToSleep()) {
        // setup an mbed timer for the next cordio timeout
        nextTimestamp = (timestamp_t)(WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

void BLE::processEvents()
{
    callDispatcher();
}

 void BLE::stack_handler(wsfEventMask_t event, wsfMsgHdr_t* msg)
 {
    if (msg == NULL) {
        return;
    }

    if (ble::pal::vendor::cordio::CordioSecurityManager::get_security_manager().sm_handler(msg)) {
        return;
    }

    switch(msg->event) {
        case DM_RESET_CMPL_IND: {
            ::BLE::InitializationCompleteCallbackContext context = {
                ::BLE::Instance(::BLE::DEFAULT_INSTANCE),
                BLE_ERROR_NONE
            };
            deviceInstance().getGattServer().initialize();
            deviceInstance().initialization_status = INITIALIZED;
            _init_callback.call(&context);
        }   break;

        default:
            ble::pal::vendor::cordio::Gap::gap_handler(msg);
            break;
    }
}

void BLE::device_manager_cb(dmEvt_t* dm_event)
{
    BLE::deviceInstance().stack_handler(0, &dm_event->hdr);
}

/*
 * AttServerInitDeInitCback callback is used to Initialize/Deinitialize
 * the CCC Table of the ATT Server when a remote peer requests to Open
 * or Close the connection.
 */
 void BLE::connection_handler(dmEvt_t* dm_event)
 {
    dmConnId_t connId = (dmConnId_t)dm_event->hdr.param;

    switch (dm_event->hdr.event) {
        case DM_CONN_OPEN_IND:
            /* set up CCC table with uninitialized (all zero) values */
            AttsCccInitTable(connId, NULL);
            break;
        case DM_CONN_CLOSE_IND:
            /* clear CCC table on connection close */
            AttsCccClearTable(connId);
            break;
        default:
            break;
    }
}

void BLE::timeoutCallback()
{
    wsf_mbed_ble_signal_event();
}

void BLE::stack_setup()
{
    MBED_ASSERT(_hci_driver != NULL);

    wsfHandlerId_t handlerId;

    buf_pool_desc_t buf_pool_desc = _hci_driver->get_buffer_pool_description();

    // Initialize buffers with the ones provided by the HCI driver
    uint16_t bytes_used = WsfBufInit(
        buf_pool_desc.buffer_size, buf_pool_desc.buffer_memory,
        buf_pool_desc.pool_count, buf_pool_desc.pool_description
    );

    MBED_ASSERT(bytes_used != 0);

    WsfTimerInit();
    SecInit();

    // Note: enable once security is supported
    SecRandInit();
    SecAesInit();
    SecCmacInit();
    SecEccInit();

    handlerId = WsfOsSetNextHandler(HciHandler);
    HciHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(DmHandler);
    DmAdvInit();
    DmScanInit();
    DmConnInit();
    DmConnMasterInit();
    DmConnSlaveInit();
    DmSecInit();
    DmPhyInit();

    // Note: enable once security is supported
    DmSecLescInit();
    DmPrivInit();
    DmHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(L2cSlaveHandler);
    L2cSlaveHandlerInit(handlerId);
    L2cInit();
    L2cSlaveInit();
    L2cMasterInit();

    handlerId = WsfOsSetNextHandler(AttHandler);
    AttHandlerInit(handlerId);
    AttsInit();
    AttsIndInit();
    AttsSignInit();
    AttsAuthorRegister(GattServer::atts_auth_cb);
    AttcInit();
    AttcSignInit();

    handlerId = WsfOsSetNextHandler(SmpHandler);
    SmpHandlerInit(handlerId);
    SmprInit();
    SmprScInit();
    SmpiInit();
    SmpiScInit();

    stack_handler_id = WsfOsSetNextHandler(&BLE::stack_handler);

    HciSetMaxRxAclLen(100);

    DmRegister(BLE::device_manager_cb);
    DmConnRegister(DM_CLIENT_ID_APP, BLE::device_manager_cb);
    AttConnRegister(BLE::connection_handler);
    AttRegister((attCback_t) ble::pal::vendor::cordio::CordioAttClient::att_client_handler);
}

void BLE::start_stack_reset()
{
    _hci_driver->initialize();
    DmDevReset();
}

void BLE::callDispatcher()
{
    // process the external event queue
    _event_queue.process();

    // follow by stack events
    static uint32_t lastTimeUs = us_ticker_read();
    uint32_t currTimeUs, deltaTimeMs;

    // Update the current cordio time
    currTimeUs = us_ticker_read();
    deltaTimeMs = (currTimeUs - lastTimeUs) / 1000;
    if (deltaTimeMs > 0) {
        WsfTimerUpdate(deltaTimeMs / WSF_MS_PER_TICK);
        lastTimeUs += deltaTimeMs * 1000;
    }

    wsfOsDispatcher();

    if (wsfOsReadyToSleep()) {
        static Timeout nextTimeout;
        // setup an mbed timer for the next Cordio timeout
        bool_t pTimerRunning;
        timestamp_t nextTimestamp = (timestamp_t) (WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

CordioHCIDriver* BLE::_hci_driver = NULL;

FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext*> BLE::_init_callback;

} // namespace cordio
} // namespace vendor
} // namespace ble
<commit_msg>BLE: Enable cordio extended modules if available.<commit_after>/* mbed Microcontroller Library
 * Copyright (c) 2017-2017 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "us_ticker_api.h"
#include "BLE.h"
#include "wsf_types.h"
#include "wsf_msg.h"
#include "wsf_os.h"
#include "wsf_buf.h"
#include "wsf_timer.h"
#include "hci_handler.h"
#include "dm_handler.h"
#include "l2c_handler.h"
#include "att_handler.h"
#include "smp_handler.h"
#include "l2c_api.h"
#include "att_api.h"
#include "smp_api.h"
#include "hci_drv.h"
#include "CordioBLE.h"
#include "mbed_assert.h"

#include "CordioPalAttClient.h"
#include "CordioPalSecurityManager.h"

/*! WSF handler ID */
wsfHandlerId_t stack_handler_id;

/**
 * Weak definition of ble_cordio_get_hci_driver.
 * A runtime error is generated if the user does not define any
 * ble_cordio_get_hci_driver.
 */
MBED_WEAK ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver()
{
    error("Please provide an implementation for the HCI driver");
    ble::vendor::cordio::CordioHCIDriver* bad_instance = NULL;
    return *bad_instance;
}

/**
 * Low level HCI interface between Cordio stack and the port.
 */
extern "C" uint16_t hci_mbed_os_drv_write(uint8_t type, uint16_t len, uint8_t *pData)
{
    return ble_cordio_get_hci_driver().write(type, len, pData);
}

extern "C" void hci_mbed_os_start_reset_sequence(void)
{
    ble_cordio_get_hci_driver().start_reset_sequence();
}

extern "C" void hci_mbed_os_handle_reset_sequence(uint8_t* msg)
{
     ble_cordio_get_hci_driver().handle_reset_sequence(msg);
}

/*
 * This function will signal to the user code by calling signalEventsToProcess.
 * It is registered and called into the Wsf Stack.
 */
extern "C" void wsf_mbed_ble_signal_event(void)
{
    ble::vendor::cordio::BLE::deviceInstance().signalEventsToProcess(::BLE::DEFAULT_INSTANCE);
}

/**
 * BLE-API requires an implementation of the following function in order to
 * obtain its transport handle.
 */
BLEInstanceBase *createBLEInstance()
{
    return (&(ble::vendor::cordio::BLE::deviceInstance()));
}

namespace ble {
namespace vendor {
namespace cordio {

BLE::BLE(CordioHCIDriver& hci_driver) :
    initialization_status(NOT_INITIALIZED),
    instanceID(::BLE::DEFAULT_INSTANCE),
    _event_queue()
{
    _hci_driver = &hci_driver;
    stack_setup();
}

BLE::~BLE() { }

/**
 * The singleton which represents the BLE transport for the BLE.
 */
BLE& BLE::deviceInstance()
{
    static BLE instance(
        ble_cordio_get_hci_driver()
    );
    return instance;
}

ble_error_t BLE::init(
    ::BLE::InstanceID_t instanceID,
    FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext *> initCallback)
{
    switch (initialization_status) {
        case NOT_INITIALIZED:
            _event_queue.initialize(this, instanceID);
            _init_callback = initCallback;
            start_stack_reset();
            return BLE_ERROR_NONE;

        case INITIALIZING:
            return BLE_ERROR_INITIALIZATION_INCOMPLETE;

        case INITIALIZED:
            return BLE_ERROR_NONE;

        default:
            return BLE_ERROR_UNSPECIFIED;
    }
}

bool BLE::hasInitialized() const
{
    return initialization_status == INITIALIZED;
}

ble_error_t BLE::shutdown()
{
    if (initialization_status != INITIALIZED) {
        return BLE_ERROR_INITIALIZATION_INCOMPLETE;
    }

    initialization_status = NOT_INITIALIZED;
    _hci_driver->terminate();

    getGattServer().reset();
    getGattClient().reset();
    getGap().reset();
    _event_queue.clear();

    return BLE_ERROR_NONE;
}

const char* BLE::getVersion()
{
    static const char version[] = "generic-cordio";
    return version;
}

generic::GenericGap& BLE::getGap()
{
    static pal::vendor::cordio::GenericAccessService cordio_gap_service;
    static ble::generic::GenericGap gap(
        _event_queue,
        pal::vendor::cordio::Gap::get_gap(),
        cordio_gap_service,
        pal::vendor::cordio::CordioSecurityManager::get_security_manager()
    );

    return gap;
}

const generic::GenericGap& BLE::getGap() const
{
    BLE &self = const_cast<BLE&>(*this);
    return const_cast<const generic::GenericGap&>(self.getGap());
};

GattServer& BLE::getGattServer()
{
    return cordio::GattServer::getInstance();
}

const GattServer& BLE::getGattServer() const
{
    return cordio::GattServer::getInstance();
}

generic::GenericGattClient& BLE::getGattClient()
{
    static pal::AttClientToGattClientAdapter pal_client(
        pal::vendor::cordio::CordioAttClient::get_client()
    );
    static generic::GenericGattClient client(&pal_client);

    return client;
}

SecurityManager& BLE::getSecurityManager()
{
    static SigningEventMonitorProxy signing_event_monitor(*this);
    static generic::GenericSecurityManager m_instance(
        pal::vendor::cordio::CordioSecurityManager::get_security_manager(),
        getGap(),
        signing_event_monitor
    );

    return m_instance;
}

const SecurityManager& BLE::getSecurityManager() const
{
    const BLE &self = const_cast<BLE&>(*this);
    return const_cast<const SecurityManager&>(self.getSecurityManager());
}

void BLE::waitForEvent()
{
    static Timeout nextTimeout;
    timestamp_t nextTimestamp;
    bool_t pTimerRunning;

    callDispatcher();

    if (wsfOsReadyToSleep()) {
        // setup an mbed timer for the next cordio timeout
        nextTimestamp = (timestamp_t)(WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

void BLE::processEvents()
{
    callDispatcher();
}

 void BLE::stack_handler(wsfEventMask_t event, wsfMsgHdr_t* msg)
 {
    if (msg == NULL) {
        return;
    }

    if (ble::pal::vendor::cordio::CordioSecurityManager::get_security_manager().sm_handler(msg)) {
        return;
    }

    switch(msg->event) {
        case DM_RESET_CMPL_IND: {
            ::BLE::InitializationCompleteCallbackContext context = {
                ::BLE::Instance(::BLE::DEFAULT_INSTANCE),
                BLE_ERROR_NONE
            };

            // initialize extended module if supported
            if (HciGetLeSupFeat() == HCI_LE_SUP_FEAT_LE_EXT_ADV) {
                DmExtAdvInit();
                DmExtScanInit();
                DmExtConnMasterInit();
                DmExtConnSlaveInit();
            }

            deviceInstance().getGattServer().initialize();
            deviceInstance().initialization_status = INITIALIZED;
            _init_callback.call(&context);
        }   break;

        default:
            ble::pal::vendor::cordio::Gap::gap_handler(msg);
            break;
    }
}

void BLE::device_manager_cb(dmEvt_t* dm_event)
{
    BLE::deviceInstance().stack_handler(0, &dm_event->hdr);
}

/*
 * AttServerInitDeInitCback callback is used to Initialize/Deinitialize
 * the CCC Table of the ATT Server when a remote peer requests to Open
 * or Close the connection.
 */
 void BLE::connection_handler(dmEvt_t* dm_event)
 {
    dmConnId_t connId = (dmConnId_t)dm_event->hdr.param;

    switch (dm_event->hdr.event) {
        case DM_CONN_OPEN_IND:
            /* set up CCC table with uninitialized (all zero) values */
            AttsCccInitTable(connId, NULL);
            break;
        case DM_CONN_CLOSE_IND:
            /* clear CCC table on connection close */
            AttsCccClearTable(connId);
            break;
        default:
            break;
    }
}

void BLE::timeoutCallback()
{
    wsf_mbed_ble_signal_event();
}

void BLE::stack_setup()
{
    MBED_ASSERT(_hci_driver != NULL);

    wsfHandlerId_t handlerId;

    buf_pool_desc_t buf_pool_desc = _hci_driver->get_buffer_pool_description();

    // Initialize buffers with the ones provided by the HCI driver
    uint16_t bytes_used = WsfBufInit(
        buf_pool_desc.buffer_size, buf_pool_desc.buffer_memory,
        buf_pool_desc.pool_count, buf_pool_desc.pool_description
    );

    MBED_ASSERT(bytes_used != 0);

    WsfTimerInit();
    SecInit();

    // Note: enable once security is supported
    SecRandInit();
    SecAesInit();
    SecCmacInit();
    SecEccInit();

    handlerId = WsfOsSetNextHandler(HciHandler);
    HciHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(DmHandler);
    DmAdvInit();
    DmScanInit();
    DmConnInit();
    DmConnMasterInit();
    DmConnSlaveInit();
    DmSecInit();
    DmPhyInit();

    // Note: enable once security is supported
    DmSecLescInit();
    DmPrivInit();
    DmHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(L2cSlaveHandler);
    L2cSlaveHandlerInit(handlerId);
    L2cInit();
    L2cSlaveInit();
    L2cMasterInit();

    handlerId = WsfOsSetNextHandler(AttHandler);
    AttHandlerInit(handlerId);
    AttsInit();
    AttsIndInit();
    AttsSignInit();
    AttsAuthorRegister(GattServer::atts_auth_cb);
    AttcInit();
    AttcSignInit();

    handlerId = WsfOsSetNextHandler(SmpHandler);
    SmpHandlerInit(handlerId);
    SmprInit();
    SmprScInit();
    SmpiInit();
    SmpiScInit();

    stack_handler_id = WsfOsSetNextHandler(&BLE::stack_handler);

    HciSetMaxRxAclLen(100);

    DmRegister(BLE::device_manager_cb);
    DmConnRegister(DM_CLIENT_ID_APP, BLE::device_manager_cb);
    AttConnRegister(BLE::connection_handler);
    AttRegister((attCback_t) ble::pal::vendor::cordio::CordioAttClient::att_client_handler);
}

void BLE::start_stack_reset()
{
    _hci_driver->initialize();
    DmDevReset();
}

void BLE::callDispatcher()
{
    // process the external event queue
    _event_queue.process();

    // follow by stack events
    static uint32_t lastTimeUs = us_ticker_read();
    uint32_t currTimeUs, deltaTimeMs;

    // Update the current cordio time
    currTimeUs = us_ticker_read();
    deltaTimeMs = (currTimeUs - lastTimeUs) / 1000;
    if (deltaTimeMs > 0) {
        WsfTimerUpdate(deltaTimeMs / WSF_MS_PER_TICK);
        lastTimeUs += deltaTimeMs * 1000;
    }

    wsfOsDispatcher();

    if (wsfOsReadyToSleep()) {
        static Timeout nextTimeout;
        // setup an mbed timer for the next Cordio timeout
        bool_t pTimerRunning;
        timestamp_t nextTimestamp = (timestamp_t) (WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

CordioHCIDriver* BLE::_hci_driver = NULL;

FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext*> BLE::_init_callback;

} // namespace cordio
} // namespace vendor
} // namespace ble
<|endoftext|>