<commit_before>/**
* @file Simulation.cpp
* @author Benjamin Schlotter
* Implementation of class Simulation
*/

#include "Simulation.h"

using namespace naoth;
using namespace std;

Simulation::Simulation()
{
  DEBUG_REQUEST_REGISTER("Simulation:draw_one_action_point:global","draw_one_action_point:global", false);
  DEBUG_REQUEST_REGISTER("Simulation:draw_ball","draw_ball", false);
  DEBUG_REQUEST_REGISTER("Simulation:ActionTarget","ActionTarget", false);
  DEBUG_REQUEST_REGISTER("Simulation:draw_best_action","best action",false);
  //DEBUG_REQUEST_REGISTER("Simulation:draw_pessimistic_best_action","best pessimistic action",false);
  DEBUG_REQUEST_REGISTER("Simulation:GoalLinePreview","GoalLinePreview",false);
  DEBUG_REQUEST_REGISTER("Simulation:draw_potential_field","Draw Potential Field",false);
  DEBUG_REQUEST_REGISTER("Simulation:use_Parameters","use_Parameters",false);

  getDebugParameterList().add(&theParameters);

  //actionRingBuffer.resize(ActionModel::numOfActions);
  //calculate the actions
  action_local.reserve(KickActionModel::numOfActions);

  action_local.push_back(Action(KickActionModel::none, Vector2d()));
  action_local.push_back(Action(KickActionModel::kick_long, Vector2d(theParameters.action_long_kick_distance, 0))); // long
  action_local.push_back(Action(KickActionModel::kick_short, Vector2d(theParameters.action_short_kick_distance, 0))); // short
  action_local.push_back(Action(KickActionModel::sidekick_right, Vector2d(0, -theParameters.action_sidekick_distance))); // right
  action_local.push_back(Action(KickActionModel::sidekick_left, Vector2d(0, theParameters.action_sidekick_distance))); // left

  actionRingBuffer.resize(KickActionModel::numOfActions);
}

Simulation::~Simulation(){}

void Simulation::execute()
{
  DEBUG_REQUEST("Simulation:use_Parameters",
  action_local.clear();
  action_local.reserve(KickActionModel::numOfActions);

  action_local.push_back(Action(KickActionModel::none, Vector2d()));
  action_local.push_back(Action(KickActionModel::kick_long, Vector2d(theParameters.action_long_kick_distance, 0))); // long
  action_local.push_back(Action(KickActionModel::kick_short, Vector2d(theParameters.action_short_kick_distance, 0))); // short
  action_local.push_back(Action(KickActionModel::sidekick_right, Vector2d(0, -theParameters.action_sidekick_distance))); // right
  action_local.push_back(Action(KickActionModel::sidekick_left, Vector2d(0, theParameters.action_sidekick_distance))); // left

  actionRingBuffer.resize(KickActionModel::numOfActions);
  );

  if(!getBallModel().valid || getFrameInfo().getTimeInSeconds() >= getBallModel().frameInfoWhenBallWasSeen.getTimeInSeconds()+1)
  {
    return;
  }
  else
  {
    int best_action = 0;
    //int pessimistic_best_action = 0;
    for(size_t i=0; i<actionRingBuffer.size(); i++)
    {
      simulate(action_local[i], actionRingBuffer[i]);

      if(action_local[i].potential < action_local[best_action].potential)
      {
        best_action = i;
      }
      //The pessimistic option
      /*if(action_local[i].pessimistPotential < action_local[best_action].pessimistPotential)
      {
        pessimistic_best_action = i;
      }*/

    }

    getKickActionModel().myAction = action_local[best_action].id();

  DEBUG_REQUEST("Simulation:draw_best_action",
  {
    FIELD_DRAWING_CONTEXT;
    PEN("FF69B4", 7);
    Vector2d actionGlobal = action_local[best_action].target;
    FILLOVAL(actionGlobal.x, actionGlobal.y, 50,50);
  });

  //DEBUG_REQUEST("Simulation:draw_pessimistic_best_action",
  //{
  //  FIELD_DRAWING_CONTEXT;
  //  PEN("0000FF", 7);
  //  Vector2d actionGlobal = action_local[pessimistic_best_action].target;
  //  FILLOVAL(actionGlobal.x, actionGlobal.y, 50,50);
  //});
  DEBUG_REQUEST("Simulation:draw_potential_field",
     draw_potential_field();
  );

  }
}//end execute

void Simulation::simulate(Simulation::Action& action, RingBufferWithSum<double, 30>& oneActionRingBuffer)const
{

  Vector2d ballPositionResult = calculateOneAction(action);

  DEBUG_REQUEST("Simulation:draw_one_action_point:global",
  {
    FIELD_DRAWING_CONTEXT;
    PEN("000000", 1);
    CIRCLE( ballPositionResult.x, ballPositionResult.y, 50);
    //TEXT_DRAWING(ballPositionResult.x,ballPositionResult.y-150,action.maximum);
    //TEXT_DRAWING(ballPositionResult.x,ballPositionResult.y-250,action.minimum);
  });

  double v = evaluateAction(ballPositionResult);

  oneActionRingBuffer.add(v);

  // HACK
  action.potential = oneActionRingBuffer.getAverage();
  action.target = ballPositionResult;
  //double s = 0;
  //int z = 0;
  ////Durch den Buffer durchgehen und alle Werte kleiner Average aufsummieren
  //for (int k=0; k < oneActionRingBuffer.size(); k++){
  //    if(oneActionRingBuffer[k] <= action.potential){
  //        s += oneActionRingBuffer[k];
  //        z++;
  //    }
  //}
  //action.pessimistPotential = s/z;
  ////Todo: als text minimum und maximum ausgeben
  //action.minimum = oneActionRingBuffer.getMinimum();

  //double max = oneActionRingBuffer[0];
  //for(int i = 0; i < oneActionRingBuffer.size();i++)
  //    {
  //      if(oneActionRingBuffer[i] > max) max = oneActionRingBuffer[i];
  //    }
  //action.maximum = max;
  //if there is big gap between our values(average and median), we know it is not good
  //action.goodness = actionRingBuffer.getAverage()/actionRingBuffer.getMedian();
}

Vector2d Simulation::calculateOneAction(const Action& action) const
{
  // STEP 1: predict the action outcome
  const Vector2d& ballRelativePreview = getBallModel().positionPreview;

  DEBUG_REQUEST("Simulation:draw_ball",
    FIELD_DRAWING_CONTEXT;
    PEN("FF0000", 1);
    Vector2d ball = getRobotPose() * getBallModel().positionPreview;
    CIRCLE( ball.x, ball.y, 50);
  );

  Vector2d result = action.predict(ballRelativePreview, theParameters.distance_variance, theParameters.angle_variance);

  DEBUG_REQUEST("Simulation:ActionTarget",
    FIELD_DRAWING_CONTEXT;
    PEN("0000FF", 1);
    Vector2d ball = getRobotPose() * result;
    CIRCLE( ball.x, ball.y, 50);
  );

  // STEP 2: calculate the goal line
  GoalModel::Goal oppGoalModel = getSelfLocGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());
  Vector2d oppGoalPostLeftPreview = getMotionStatus().plannedMotion.hip / oppGoalModel.leftPost;
  Vector2d oppGoalPostRightPreview = getMotionStatus().plannedMotion.hip / oppGoalModel.rightPost;

  //the endpoints of our line are a shortened version of the goal line
  Vector2d goalDir = (oppGoalPostRightPreview - oppGoalPostLeftPreview).normalize();
  Vector2d leftEndpoint = oppGoalPostLeftPreview + goalDir*(getFieldInfo().goalpostRadius + getFieldInfo().ballRadius);
  Vector2d rightEndpoint = oppGoalPostRightPreview - goalDir*(getFieldInfo().goalpostRadius + getFieldInfo().ballRadius);

  // this is the goalline we are shooting for
  Math::LineSegment goalLinePreview(leftEndpoint, rightEndpoint);

  // note: the drawing is global
  DEBUG_REQUEST("Simulation:GoalLinePreview",
    FIELD_DRAWING_CONTEXT;
    PEN("000000", 1);
    Vector2d globalLeft = getRobotPose() * leftEndpoint;
    Vector2d globalRight = getRobotPose() * rightEndpoint;
    LINE(globalLeft.x,globalLeft.y,globalRight.x,globalRight.y);
  );

  // STEP 3: estimate external factors (shooting a goal, ball moved by referee)
  //Math::LineSegment shootLine(ballRelativePreview, outsideField(lonely_action.target));//hmm
  Math::LineSegment shootLine(ballRelativePreview, result);

  Vector2d resultingBallPosition;

  if(shootLine.intersect(goalLinePreview) && goalLinePreview.intersect(shootLine)) {
    resultingBallPosition = Vector2d(getFieldInfo().xPosOpponentGoal+200, 0.0);
  } else {
    resultingBallPosition = outsideField(getRobotPose() * result);
  }

  return resultingBallPosition;
}

  //correction of distance in percentage, angle in degrees
  Vector2d Simulation::Action::predict(const Vector2d& ball, double distance_variance, double angle_variance) const
  {
  double random_length = 2.0*Math::random()-1.0;
  double random_angle = 2.0*Math::random()-1.0;

  Vector2d noisyAction = actionVector + actionVector*(distance_variance*random_length);
  noisyAction.rotate(angle_variance*random_angle);

  return ball + noisyAction;
  }

//calcualte according to the rules, without the roboter position, the ball position
//if it goes outside the field
Vector2d Simulation::outsideField(const Vector2d& globalPoint) const
{
  Vector2d point = globalPoint;

  //Schusslinie
  Math::LineSegment shootLine(getBallModel().positionPreview, globalPoint);

  if(getFieldInfo().fieldRect.inside(point)){
    return point;
  }
  else
      //Nach Prioritten geordnet - zuerst die Regeln mit dem mglichst schlimmsten Resultat
  {   //Opponent Groundline Out - Ball einen Meter hinter Roboter mind ansto hhe. jeweils seite wo ins ausgeht
      if(point.x > getFieldInfo().xPosOpponentGroundline)
      {
        Vector2d OppOutPoint = getFieldInfo().oppLineSegment.point(getFieldInfo().oppLineSegment.intersection(shootLine));
        if(OppOutPoint.y < 0)
        {
          return Vector2d(0, getFieldInfo().yThrowInLineRight);//range check
        } else
        {
          return Vector2d(0, getFieldInfo().yThrowInLineLeft);
        }
      }
      //Own Groundline out -  an der seite wo raus geht
      else if(point.x < getFieldInfo().xPosOwnGroundline)
      {
        Vector2d OwnOutPoint = getFieldInfo().ownLineSegment.point(getFieldInfo().ownLineSegment.intersection(shootLine));
        if(OwnOutPoint.y < 0)
        {
          return Vector2d(getFieldInfo().xThrowInLineOwn, getFieldInfo().yThrowInLineRight);//range check
        } else
        {
          return Vector2d(getFieldInfo().xThrowInLineOwn, getFieldInfo().yThrowInLineLeft);
        }
      }
      //an der linken Seite raus -> ein meter hinter roboter oder wo ins ausgeht ein meter hinter
      else if(point.y > getFieldInfo().yPosLeftSideline )
      {
        Vector2d leftOutPoint = getFieldInfo().leftLineSegment.point(getFieldInfo().leftLineSegment.intersection(shootLine));
        point.x = min(leftOutPoint.x,getRobotPose().translation.x);

        if(point.x-1000 < getFieldInfo().xThrowInLineOwn)
        {
          point.x = getFieldInfo().xThrowInLineOwn;
        } else
        {
          point.x -= 1000;
        }
        return Vector2d(point.x, getFieldInfo().yThrowInLineLeft); //range check
      }
      //an der rechten Seite raus -> ein meter hinter roboter oder wo ins ausgeht ein meter hinter
      else //(point.y < getFieldInfo().yPosRightSideline)
      {
        Vector2d rightOutPoint = getFieldInfo().rightLineSegment.point(getFieldInfo().rightLineSegment.intersection(shootLine));
        point.x = min(rightOutPoint.x,getRobotPose().translation.x);

        if(point.x-1000 < getFieldInfo().xThrowInLineOwn)
        {
        point.x = getFieldInfo().xThrowInLineOwn;
        } else
        {
         point.x -= 1000;
        }
      return Vector2d(point.x, getFieldInfo().yThrowInLineRight);//range check
      }
  }
}

double Simulation::evaluateAction(const Vector2d& a) const{
  Vector2d oppGoal(getFieldInfo().xPosOpponentGoal+200, 0.0);
  Vector2d oppDiff = oppGoal - a;

  double oppValueX = 0.1;
  double oppValueY = 1;
  MODIFY("Simulation:oppValueX", oppValueX);
  MODIFY("Simulation:oppValueY", oppValueY);
  double value_opp = oppValueX*oppDiff.x*oppDiff.x + oppValueY*oppDiff.y*oppDiff.y;

  //Vector2d ownGoal(getFieldInfo().xPosOwnGoal, 0.0);
  //Vector2d ownDiff = ownGoal - a;

  //double ownValueX = 0.01;
  //double ownValueY = 0.1;
  //MODIFY("Simulation:ownValueX", ownValueX);
  //MODIFY("Simulation:ownValueY", ownValueY);
  //double value_own = ownValueX*ownDiff.x*ownDiff.x + ownValueY*ownDiff.y*ownDiff.y;

  //return value_opp - value_own;
  return value_opp;
}

void Simulation::draw_potential_field() const
{
  static const int ySize = 20;
  static const int xSize = 30;
  double yWidth = 0.5*getFieldInfo().yFieldLength/(double)ySize;
  double xWidth = 0.5*getFieldInfo().xFieldLength/(double)xSize;

  FIELD_DRAWING_CONTEXT;
  Color white(0.0,0.0,1.0,0.5); // transparent
  Color black(1.0,0.0,0.0,0.5);

  // create new sample set
  std::vector<double> potential(xSize*ySize);
  int idx = 0;

  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2d point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));
      potential[idx] = evaluateAction(point);
      idx++;
    }
  }

  double maxValue = 0;
  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      maxValue = max(maxValue, potential[idx++]);
    }
  }

  if(maxValue == 0) return;

  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2d point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));

      double t = potential[idx++] / maxValue;
      Color color = black*t + white*(1-t);
      PEN(color, 20);
      FILLBOX(point.x - xWidth, point.y - yWidth, point.x+xWidth, point.y+yWidth);
    }
  }
}//end draw_closest_points<commit_msg>Cleanup DebugRequest<commit_after>/**
* @file Simulation.cpp
* @author Benjamin Schlotter
* Implementation of class Simulation
*/

#include "Simulation.h"

using namespace naoth;
using namespace std;

Simulation::Simulation()
{
  DEBUG_REQUEST_REGISTER("Simulation:draw_one_action_point:global","draw_one_action_point:global", false);
  DEBUG_REQUEST_REGISTER("Simulation:draw_ball","draw_ball", false);
  DEBUG_REQUEST_REGISTER("Simulation:ActionTarget","ActionTarget", false);
  DEBUG_REQUEST_REGISTER("Simulation:draw_best_action","best action",false);
  DEBUG_REQUEST_REGISTER("Simulation:GoalLinePreview","GoalLinePreview",false);
  DEBUG_REQUEST_REGISTER("Simulation:draw_potential_field","Draw Potential Field",false);
  DEBUG_REQUEST_REGISTER("Simulation:use_Parameters","use_Parameters",false);

  getDebugParameterList().add(&theParameters);

  //calculate the actions
  action_local.reserve(KickActionModel::numOfActions);

  action_local.push_back(Action(KickActionModel::none, Vector2d()));
  action_local.push_back(Action(KickActionModel::kick_long, Vector2d(theParameters.action_long_kick_distance, 0))); // long
  action_local.push_back(Action(KickActionModel::kick_short, Vector2d(theParameters.action_short_kick_distance, 0))); // short
  action_local.push_back(Action(KickActionModel::sidekick_right, Vector2d(0, -theParameters.action_sidekick_distance))); // right
  action_local.push_back(Action(KickActionModel::sidekick_left, Vector2d(0, theParameters.action_sidekick_distance))); // left

  actionRingBuffer.resize(KickActionModel::numOfActions);
}

Simulation::~Simulation(){}

void Simulation::execute()
{
  DEBUG_REQUEST("Simulation:use_Parameters",
  action_local.clear();
  action_local.reserve(KickActionModel::numOfActions);

  action_local.push_back(Action(KickActionModel::none, Vector2d()));
  action_local.push_back(Action(KickActionModel::kick_long, Vector2d(theParameters.action_long_kick_distance, 0))); // long
  action_local.push_back(Action(KickActionModel::kick_short, Vector2d(theParameters.action_short_kick_distance, 0))); // short
  action_local.push_back(Action(KickActionModel::sidekick_right, Vector2d(0, -theParameters.action_sidekick_distance))); // right
  action_local.push_back(Action(KickActionModel::sidekick_left, Vector2d(0, theParameters.action_sidekick_distance))); // left

  actionRingBuffer.resize(KickActionModel::numOfActions);
  );

  if(!getBallModel().valid || getFrameInfo().getTimeInSeconds() >= getBallModel().frameInfoWhenBallWasSeen.getTimeInSeconds()+1)
  {
    return;
  }
  else
  {
    int best_action = 0;
    for(size_t i=0; i<actionRingBuffer.size(); i++)
    {
      simulate(action_local[i], actionRingBuffer[i]);

      if(action_local[i].potential < action_local[best_action].potential)
      {
        best_action = i;
      }
    }

    getKickActionModel().myAction = action_local[best_action].id();

  DEBUG_REQUEST("Simulation:draw_best_action",
  {
    FIELD_DRAWING_CONTEXT;
    PEN("FF69B4", 7);
    Vector2d actionGlobal = action_local[best_action].target;
    FILLOVAL(actionGlobal.x, actionGlobal.y, 50,50);
  });

  DEBUG_REQUEST("Simulation:draw_potential_field",
     draw_potential_field();
  );

  }
}//end execute

void Simulation::simulate(Simulation::Action& action, RingBufferWithSum<double, 30>& oneActionRingBuffer)const
{
  Vector2d ballPositionResult = calculateOneAction(action);

  DEBUG_REQUEST("Simulation:draw_one_action_point:global",
  {
    FIELD_DRAWING_CONTEXT;
    PEN("000000", 1);
    CIRCLE( ballPositionResult.x, ballPositionResult.y, 50);
  });

  double v = evaluateAction(ballPositionResult);

  oneActionRingBuffer.add(v);

  // HACK
  action.potential = oneActionRingBuffer.getAverage();
  action.target = ballPositionResult;
}

Vector2d Simulation::calculateOneAction(const Action& action) const
{
  // STEP 1: predict the action outcome
  const Vector2d& ballRelativePreview = getBallModel().positionPreview;

  DEBUG_REQUEST("Simulation:draw_ball",
    FIELD_DRAWING_CONTEXT;
    PEN("FF0000", 1);
    Vector2d ball = getRobotPose() * getBallModel().positionPreview;
    CIRCLE( ball.x, ball.y, 50);
  );

  Vector2d result = action.predict(ballRelativePreview, theParameters.distance_variance, theParameters.angle_variance);

  DEBUG_REQUEST("Simulation:ActionTarget",
    FIELD_DRAWING_CONTEXT;
    PEN("0000FF", 1);
    Vector2d ball = getRobotPose() * result;
    CIRCLE( ball.x, ball.y, 50);
  );

  // STEP 2: calculate the goal line
  GoalModel::Goal oppGoalModel = getSelfLocGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());
  Vector2d oppGoalPostLeftPreview = getMotionStatus().plannedMotion.hip / oppGoalModel.leftPost;
  Vector2d oppGoalPostRightPreview = getMotionStatus().plannedMotion.hip / oppGoalModel.rightPost;

  //the endpoints of our line are a shortened version of the goal line
  Vector2d goalDir = (oppGoalPostRightPreview - oppGoalPostLeftPreview).normalize();
  Vector2d leftEndpoint = oppGoalPostLeftPreview + goalDir*(getFieldInfo().goalpostRadius + getFieldInfo().ballRadius);
  Vector2d rightEndpoint = oppGoalPostRightPreview - goalDir*(getFieldInfo().goalpostRadius + getFieldInfo().ballRadius);

  // this is the goalline we are shooting for
  Math::LineSegment goalLinePreview(leftEndpoint, rightEndpoint);

  // note: the drawing is global
  DEBUG_REQUEST("Simulation:GoalLinePreview",
    FIELD_DRAWING_CONTEXT;
    PEN("000000", 1);
    Vector2d globalLeft = getRobotPose() * leftEndpoint;
    Vector2d globalRight = getRobotPose() * rightEndpoint;
    LINE(globalLeft.x,globalLeft.y,globalRight.x,globalRight.y);
  );

  // STEP 3: estimate external factors (shooting a goal, ball moved by referee)
  //Math::LineSegment shootLine(ballRelativePreview, outsideField(lonely_action.target));//hmm
  Math::LineSegment shootLine(ballRelativePreview, result);

  Vector2d resultingBallPosition;

  if(shootLine.intersect(goalLinePreview) && goalLinePreview.intersect(shootLine)) {
    resultingBallPosition = Vector2d(getFieldInfo().xPosOpponentGoal+200, 0.0);
  } else {
    resultingBallPosition = outsideField(getRobotPose() * result);
  }

  return resultingBallPosition;
}

  //correction of distance in percentage, angle in degrees
  Vector2d Simulation::Action::predict(const Vector2d& ball, double distance_variance, double angle_variance) const
  {
  double random_length = 2.0*Math::random()-1.0;
  double random_angle = 2.0*Math::random()-1.0;

  Vector2d noisyAction = actionVector + actionVector*(distance_variance*random_length);
  noisyAction.rotate(angle_variance*random_angle);

  return ball + noisyAction;
  }

//calcualte according to the rules, without the roboter position, the ball position
//if it goes outside the field
Vector2d Simulation::outsideField(const Vector2d& globalPoint) const
{
  Vector2d point = globalPoint;

  //Schusslinie
  Math::LineSegment shootLine(getBallModel().positionPreview, globalPoint);

  if(getFieldInfo().fieldRect.inside(point)){
    return point;
  }
  else
      //Nach Prioritten geordnet - zuerst die Regeln mit dem mglichst schlimmsten Resultat
  {   //Opponent Groundline Out - Ball einen Meter hinter Roboter mind ansto hhe. jeweils seite wo ins ausgeht
      if(point.x > getFieldInfo().xPosOpponentGroundline)
      {
        Vector2d OppOutPoint = getFieldInfo().oppLineSegment.point(getFieldInfo().oppLineSegment.intersection(shootLine));
        if(OppOutPoint.y < 0)
        {
          return Vector2d(0, getFieldInfo().yThrowInLineRight);//range check
        } else
        {
          return Vector2d(0, getFieldInfo().yThrowInLineLeft);
        }
      }
      //Own Groundline out -  an der seite wo raus geht
      else if(point.x < getFieldInfo().xPosOwnGroundline)
      {
        Vector2d OwnOutPoint = getFieldInfo().ownLineSegment.point(getFieldInfo().ownLineSegment.intersection(shootLine));
        if(OwnOutPoint.y < 0)
        {
          return Vector2d(getFieldInfo().xThrowInLineOwn, getFieldInfo().yThrowInLineRight);//range check
        } else
        {
          return Vector2d(getFieldInfo().xThrowInLineOwn, getFieldInfo().yThrowInLineLeft);
        }
      }
      //an der linken Seite raus -> ein meter hinter roboter oder wo ins ausgeht ein meter hinter
      else if(point.y > getFieldInfo().yPosLeftSideline )
      {
        Vector2d leftOutPoint = getFieldInfo().leftLineSegment.point(getFieldInfo().leftLineSegment.intersection(shootLine));
        point.x = min(leftOutPoint.x,getRobotPose().translation.x);

        if(point.x-1000 < getFieldInfo().xThrowInLineOwn)
        {
          point.x = getFieldInfo().xThrowInLineOwn;
        } else
        {
          point.x -= 1000;
        }
        return Vector2d(point.x, getFieldInfo().yThrowInLineLeft); //range check
      }
      //an der rechten Seite raus -> ein meter hinter roboter oder wo ins ausgeht ein meter hinter
      else //(point.y < getFieldInfo().yPosRightSideline)
      {
        Vector2d rightOutPoint = getFieldInfo().rightLineSegment.point(getFieldInfo().rightLineSegment.intersection(shootLine));
        point.x = min(rightOutPoint.x,getRobotPose().translation.x);

        if(point.x-1000 < getFieldInfo().xThrowInLineOwn)
        {
        point.x = getFieldInfo().xThrowInLineOwn;
        } else
        {
         point.x -= 1000;
        }
      return Vector2d(point.x, getFieldInfo().yThrowInLineRight);//range check
      }
  }
}

double Simulation::evaluateAction(const Vector2d& a) const{
  Vector2d oppGoal(getFieldInfo().xPosOpponentGoal+200, 0.0);
  Vector2d oppDiff = oppGoal - a;

  double oppValueX = 0.1;
  double oppValueY = 1;
  MODIFY("Simulation:oppValueX", oppValueX);
  MODIFY("Simulation:oppValueY", oppValueY);
  double value_opp = oppValueX*oppDiff.x*oppDiff.x + oppValueY*oppDiff.y*oppDiff.y;

  //Vector2d ownGoal(getFieldInfo().xPosOwnGoal, 0.0);
  //Vector2d ownDiff = ownGoal - a;

  //double ownValueX = 0.01;
  //double ownValueY = 0.1;
  //MODIFY("Simulation:ownValueX", ownValueX);
  //MODIFY("Simulation:ownValueY", ownValueY);
  //double value_own = ownValueX*ownDiff.x*ownDiff.x + ownValueY*ownDiff.y*ownDiff.y;

  //return value_opp - value_own;
  return value_opp;
}

void Simulation::draw_potential_field() const
{
  static const int ySize = 20;
  static const int xSize = 30;
  double yWidth = 0.5*getFieldInfo().yFieldLength/(double)ySize;
  double xWidth = 0.5*getFieldInfo().xFieldLength/(double)xSize;

  FIELD_DRAWING_CONTEXT;
  Color white(0.0,0.0,1.0,0.5); // transparent
  Color black(1.0,0.0,0.0,0.5);

  // create new sample set
  std::vector<double> potential(xSize*ySize);
  int idx = 0;

  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2d point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));
      potential[idx] = evaluateAction(point);
      idx++;
    }
  }

  double maxValue = 0;
  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      maxValue = max(maxValue, potential[idx++]);
    }
  }

  if(maxValue == 0) return;

  idx = 0;
  for (int x = 0; x < xSize; x++) {
    for (int y = 0; y < ySize; y++)
    {
      Vector2d point(xWidth*(2*x-xSize+1), yWidth*(2*y-ySize+1));

      double t = potential[idx++] / maxValue;
      Color color = black*t + white*(1-t);
      PEN(color, 20);
      FILLBOX(point.x - xWidth, point.y - yWidth, point.x+xWidth, point.y+yWidth);
    }
  }
}//end draw_closest_points<|endoftext|>


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

/* @file
 * Device model for Intel's 8254x line of gigabit ethernet controllers.
 * In particular an 82547 revision 2 (82547GI) MAC because it seems to have the
 * fewest workarounds in the driver. It will probably work with most of the
 * other MACs with slight modifications.
 */

#include "base/inet.hh"
#include "dev/i8254xGBe.hh"
#include "mem/packet.hh"
#include "sim/builder.hh"
#include "sim/stats.hh"
#include "sim/system.hh"

using namespace iGbReg;

IGbE::IGbE(Params *p)
    : PciDev(p), etherInt(NULL)
{
    // Initialized internal registers per Intel documentation
    regs.tctl.reg       = 0;
    regs.rctl.reg       = 0;
    regs.ctrl.reg       = 0;
    regs.ctrl.fd        = 1;
    regs.ctrl.lrst      = 1;
    regs.ctrl.speed     = 2;
    regs.ctrl.frcspd    = 1;
    regs.sts.reg        = 0;
    regs.eecd.reg       = 0;
    regs.eecd.fwe       = 1;
    regs.eecd.ee_type   = 1;
    regs.eerd.reg       = 0;
    regs.icd.reg        = 0;
    regs.imc.reg        = 0;
    regs.rctl.reg       = 0;
    regs.tctl.reg       = 0;
    regs.manc.reg       = 0;

    eeOpBits            = 0;
    eeAddrBits          = 0;
    eeDataBits          = 0;
    eeOpcode            = 0;

    memset(&flash, 0, EEPROM_SIZE);
    // Magic happy checksum value
    flash[0] = 0xBABA;
}

Tick
IGbE::writeConfig(PacketPtr pkt)
{
    int offset = pkt->getAddr() & PCI_CONFIG_SIZE;
    if (offset < PCI_DEVICE_SPECIFIC)
        PciDev::writeConfig(pkt);
    else
        panic("Device specific PCI config space not implemented.\n");

    ///
    /// Some work may need to be done here based for the pci COMMAND bits.
    ///

    return pioDelay;
}

Tick
IGbE::read(PacketPtr pkt)
{
    int bar;
    Addr daddr;

    if (!getBAR(pkt->getAddr(), bar, daddr))
        panic("Invalid PCI memory access to unmapped memory.\n");

    // Only Memory register BAR is allowed
    assert(bar == 0);

    // Only 32bit accesses allowed
    assert(pkt->getSize() == 4);

    DPRINTF(Ethernet, "Read device register %#X\n", daddr);

    pkt->allocate();

    ///
    /// Handle read of register here
    ///

    switch (daddr) {
      case CTRL:
       pkt->set<uint32_t>(regs.ctrl.reg);
       break;
      case STATUS:
       pkt->set<uint32_t>(regs.sts.reg);
       break;
      case EECD:
       pkt->set<uint32_t>(regs.eecd.reg);
       break;
      case EERD:
       pkt->set<uint32_t>(regs.eerd.reg);
       break;
      case ICR:
       pkt->set<uint32_t>(regs.icd.reg);
       break;
      case IMC:
       pkt->set<uint32_t>(regs.imc.reg);
       break;
      case RCTL:
       pkt->set<uint32_t>(regs.rctl.reg);
       break;
      case TCTL:
       pkt->set<uint32_t>(regs.tctl.reg);
       break;
      case MANC:
       pkt->set<uint32_t>(regs.manc.reg);
       break;
      default:
       panic("Read request to unknown register number: %#x\n", daddr);
    };

    pkt->result = Packet::Success;
    return pioDelay;
}

Tick
IGbE::write(PacketPtr pkt)
{
    int bar;
    Addr daddr;

    if (!getBAR(pkt->getAddr(), bar, daddr))
        panic("Invalid PCI memory access to unmapped memory.\n");

    // Only Memory register BAR is allowed
    assert(bar == 0);

    // Only 32bit accesses allowed
    assert(pkt->getSize() == sizeof(uint32_t));

    DPRINTF(Ethernet, "Wrote device register %#X value %#X\n", daddr, pkt->get<uint32_t>());

    ///
    /// Handle write of register here
    ///
    uint32_t val = pkt->get<uint32_t>();

    switch (daddr) {
      case CTRL:
       regs.ctrl.reg = val;
       break;
      case STATUS:
       regs.sts.reg = val;
       break;
      case EECD:
       int oldClk;
       oldClk = regs.eecd.sk;
       regs.eecd.reg = val;
       // See if this is a eeprom access and emulate accordingly
       if (!oldClk && regs.eecd.sk) {
           if (eeOpBits < 8) {
               eeOpcode = eeOpcode << 1 | regs.eecd.din;
               eeOpBits++;
           } else if (eeAddrBits < 8 && eeOpcode == EEPROM_READ_OPCODE_SPI) {
               eeAddr = eeAddr << 1 | regs.eecd.din;
               eeAddrBits++;
           } else if (eeDataBits < 16 && eeOpcode == EEPROM_READ_OPCODE_SPI) {
               assert(eeAddr < EEPROM_SIZE);
               DPRINTF(Ethernet, "EEPROM bit read: %d word: %#X\n",
                       flash[eeAddr] >> eeDataBits & 0x1, flash[eeAddr]);
               regs.eecd.dout = (flash[eeAddr] >> eeDataBits) & 0x1;
               eeDataBits++;
           } else if (eeDataBits < 8 && eeOpcode == EEPROM_RDSR_OPCODE_SPI) {
               regs.eecd.dout = 0;
               eeDataBits++;
           } else
               panic("What's going on with eeprom interface? opcode:"
                      " %#x:%d addr: %#x:%d, data: %d\n", (uint32_t)eeOpcode,
                      (uint32_t)eeOpBits, (uint32_t)eeAddr,
                      (uint32_t)eeAddrBits, (uint32_t)eeDataBits);

           // Reset everything for the next command
           if ((eeDataBits == 16 && eeOpcode == EEPROM_READ_OPCODE_SPI) ||
               (eeDataBits == 8 && eeOpcode == EEPROM_RDSR_OPCODE_SPI)) {
               eeOpBits = 0;
               eeAddrBits = 0;
               eeDataBits = 0;
               eeOpcode = 0;
               eeAddr = 0;
           }

           DPRINTF(Ethernet, "EEPROM: opcode: %#X:%d\n",
                   (uint32_t)eeOpcode, (uint32_t) eeOpBits);
           if (eeOpBits == 8 && !(eeOpcode == EEPROM_READ_OPCODE_SPI ||
                                  eeOpcode == EEPROM_RDSR_OPCODE_SPI ))
               panic("Unknown eeprom opcode: %#X:%d\n", (uint32_t)eeOpcode,
                       (uint32_t)eeOpBits);

       }
       // If driver requests eeprom access, immediately give it to it
       regs.eecd.ee_gnt = regs.eecd.ee_req;
       break;
      case EERD:
       regs.eerd.reg = val;
       break;
      case ICR:
       regs.icd.reg = val;
       break;
      case IMC:
       regs.imc.reg = val;
       break;
      case RCTL:
       regs.rctl.reg = val;
       break;
      case TCTL:
       regs.tctl.reg = val;
       break;
      case MANC:
       regs.manc.reg = val;
       break;
      default:
       panic("Write request to unknown register number: %#x\n", daddr);
    };

    pkt->result = Packet::Success;
    return pioDelay;
}

bool
IGbE::ethRxPkt(EthPacketPtr packet)
{
    panic("Need to implemenet\n");
}

void
IGbE::ethTxDone()
{
    panic("Need to implemenet\n");
}

void
IGbE::serialize(std::ostream &os)
{
    panic("Need to implemenet\n");
}

void
IGbE::unserialize(Checkpoint *cp, const std::string &section)
{
    panic("Need to implemenet\n");
}

BEGIN_DECLARE_SIM_OBJECT_PARAMS(IGbEInt)

    SimObjectParam<EtherInt *> peer;
    SimObjectParam<IGbE *> device;

END_DECLARE_SIM_OBJECT_PARAMS(IGbEInt)

BEGIN_INIT_SIM_OBJECT_PARAMS(IGbEInt)

    INIT_PARAM_DFLT(peer, "peer interface", NULL),
    INIT_PARAM(device, "Ethernet device of this interface")

END_INIT_SIM_OBJECT_PARAMS(IGbEInt)

CREATE_SIM_OBJECT(IGbEInt)
{
    IGbEInt *dev_int = new IGbEInt(getInstanceName(), device);

    EtherInt *p = (EtherInt *)peer;
    if (p) {
        dev_int->setPeer(p);
        p->setPeer(dev_int);
    }

    return dev_int;
}

REGISTER_SIM_OBJECT("IGbEInt", IGbEInt)

BEGIN_DECLARE_SIM_OBJECT_PARAMS(IGbE)

    SimObjectParam<System *> system;
    SimObjectParam<Platform *> platform;
    SimObjectParam<PciConfigData *> configdata;
    Param<uint32_t> pci_bus;
    Param<uint32_t> pci_dev;
    Param<uint32_t> pci_func;
    Param<Tick> pio_latency;
    Param<Tick> config_latency;

END_DECLARE_SIM_OBJECT_PARAMS(IGbE)

BEGIN_INIT_SIM_OBJECT_PARAMS(IGbE)

    INIT_PARAM(system, "System pointer"),
    INIT_PARAM(platform, "Platform pointer"),
    INIT_PARAM(configdata, "PCI Config data"),
    INIT_PARAM(pci_bus, "PCI bus ID"),
    INIT_PARAM(pci_dev, "PCI device number"),
    INIT_PARAM(pci_func, "PCI function code"),
    INIT_PARAM_DFLT(pio_latency, "Programmed IO latency in bus cycles", 1),
    INIT_PARAM(config_latency, "Number of cycles for a config read or write")

END_INIT_SIM_OBJECT_PARAMS(IGbE)

CREATE_SIM_OBJECT(IGbE)
{
    IGbE::Params *params = new IGbE::Params;

    params->name = getInstanceName();
    params->platform = platform;
    params->system = system;
    params->configData = configdata;
    params->busNum = pci_bus;
    params->deviceNum = pci_dev;
    params->functionNum = pci_func;
    params->pio_delay = pio_latency;
    params->config_delay = config_latency;

    return new IGbE(params);
}

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

/* @file
 * Device model for Intel's 8254x line of gigabit ethernet controllers.
 * In particular an 82547 revision 2 (82547GI) MAC because it seems to have the
 * fewest workarounds in the driver. It will probably work with most of the
 * other MACs with slight modifications.
 */

#include "base/inet.hh"
#include "dev/i8254xGBe.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"
#include "sim/builder.hh"
#include "sim/stats.hh"
#include "sim/system.hh"

using namespace iGbReg;

IGbE::IGbE(Params *p)
    : PciDev(p), etherInt(NULL)
{
    // Initialized internal registers per Intel documentation
    regs.tctl.reg       = 0;
    regs.rctl.reg       = 0;
    regs.ctrl.reg       = 0;
    regs.ctrl.fd        = 1;
    regs.ctrl.lrst      = 1;
    regs.ctrl.speed     = 2;
    regs.ctrl.frcspd    = 1;
    regs.sts.reg        = 0;
    regs.eecd.reg       = 0;
    regs.eecd.fwe       = 1;
    regs.eecd.ee_type   = 1;
    regs.eerd.reg       = 0;
    regs.icd.reg        = 0;
    regs.imc.reg        = 0;
    regs.rctl.reg       = 0;
    regs.tctl.reg       = 0;
    regs.manc.reg       = 0;

    regs.pba.rxa        = 0x30;
    regs.pba.txa        = 0x10;

    eeOpBits            = 0;
    eeAddrBits          = 0;
    eeDataBits          = 0;
    eeOpcode            = 0;

    // clear all 64 16 bit words of the eeprom
    memset(&flash, 0, EEPROM_SIZE*2);

    // Magic happy checksum value
    flash[0] = 0xBABA;
}

Tick
IGbE::writeConfig(PacketPtr pkt)
{
    int offset = pkt->getAddr() & PCI_CONFIG_SIZE;
    if (offset < PCI_DEVICE_SPECIFIC)
        PciDev::writeConfig(pkt);
    else
        panic("Device specific PCI config space not implemented.\n");

    ///
    /// Some work may need to be done here based for the pci COMMAND bits.
    ///

    return pioDelay;
}

Tick
IGbE::read(PacketPtr pkt)
{
    int bar;
    Addr daddr;

    if (!getBAR(pkt->getAddr(), bar, daddr))
        panic("Invalid PCI memory access to unmapped memory.\n");

    // Only Memory register BAR is allowed
    assert(bar == 0);

    // Only 32bit accesses allowed
    assert(pkt->getSize() == 4);

    //DPRINTF(Ethernet, "Read device register %#X\n", daddr);

    pkt->allocate();

    ///
    /// Handle read of register here
    ///

    switch (daddr) {
      case CTRL:
       pkt->set<uint32_t>(regs.ctrl.reg);
       break;
      case STATUS:
       pkt->set<uint32_t>(regs.sts.reg);
       break;
      case EECD:
       pkt->set<uint32_t>(regs.eecd.reg);
       break;
      case EERD:
       pkt->set<uint32_t>(regs.eerd.reg);
       break;
      case ICR:
       pkt->set<uint32_t>(regs.icd.reg);
       break;
      case IMC:
       pkt->set<uint32_t>(regs.imc.reg);
       break;
      case RCTL:
       pkt->set<uint32_t>(regs.rctl.reg);
       break;
      case TCTL:
       pkt->set<uint32_t>(regs.tctl.reg);
       break;
      case PBA:
       pkt->set<uint32_t>(regs.pba.reg);
       break;
      case WUC:
      case LEDCTL:
       pkt->set<uint32_t>(0); // We don't care, so just return 0
       break;
      case MANC:
       pkt->set<uint32_t>(regs.manc.reg);
       break;
      default:
       if (!(daddr >= VFTA && daddr < (VFTA + VLAN_FILTER_TABLE_SIZE)*4) &&
           !(daddr >= RAL && daddr < (RAL + RCV_ADDRESS_TABLE_SIZE)*4) &&
           !(daddr >= MTA && daddr < (MTA + MULTICAST_TABLE_SIZE)*4))
           pkt->set<uint32_t>(0);
       else
           panic("Read request to unknown register number: %#x\n", daddr);
    };

    pkt->result = Packet::Success;
    return pioDelay;
}

Tick
IGbE::write(PacketPtr pkt)
{
    int bar;
    Addr daddr;

    if (!getBAR(pkt->getAddr(), bar, daddr))
        panic("Invalid PCI memory access to unmapped memory.\n");

    // Only Memory register BAR is allowed
    assert(bar == 0);

    // Only 32bit accesses allowed
    assert(pkt->getSize() == sizeof(uint32_t));

    //DPRINTF(Ethernet, "Wrote device register %#X value %#X\n", daddr, pkt->get<uint32_t>());

    ///
    /// Handle write of register here
    ///
    uint32_t val = pkt->get<uint32_t>();

    switch (daddr) {
      case CTRL:
       regs.ctrl.reg = val;
       break;
      case STATUS:
       regs.sts.reg = val;
       break;
      case EECD:
       int oldClk;
       oldClk = regs.eecd.sk;
       regs.eecd.reg = val;
       // See if this is a eeprom access and emulate accordingly
       if (!oldClk && regs.eecd.sk) {
           if (eeOpBits < 8) {
               eeOpcode = eeOpcode << 1 | regs.eecd.din;
               eeOpBits++;
           } else if (eeAddrBits < 8 && eeOpcode == EEPROM_READ_OPCODE_SPI) {
               eeAddr = eeAddr << 1 | regs.eecd.din;
               eeAddrBits++;
           } else if (eeDataBits < 16 && eeOpcode == EEPROM_READ_OPCODE_SPI) {
               assert(eeAddr>>1 < EEPROM_SIZE);
               DPRINTF(EthernetEEPROM, "EEPROM bit read: %d word: %#X\n",
                       flash[eeAddr>>1] >> eeDataBits & 0x1, flash[eeAddr>>1]);
               regs.eecd.dout = (flash[eeAddr>>1] >> (15-eeDataBits)) & 0x1;
               eeDataBits++;
           } else if (eeDataBits < 8 && eeOpcode == EEPROM_RDSR_OPCODE_SPI) {
               regs.eecd.dout = 0;
               eeDataBits++;
           } else
               panic("What's going on with eeprom interface? opcode:"
                      " %#x:%d addr: %#x:%d, data: %d\n", (uint32_t)eeOpcode,
                      (uint32_t)eeOpBits, (uint32_t)eeAddr,
                      (uint32_t)eeAddrBits, (uint32_t)eeDataBits);

           // Reset everything for the next command
           if ((eeDataBits == 16 && eeOpcode == EEPROM_READ_OPCODE_SPI) ||
               (eeDataBits == 8 && eeOpcode == EEPROM_RDSR_OPCODE_SPI)) {
               eeOpBits = 0;
               eeAddrBits = 0;
               eeDataBits = 0;
               eeOpcode = 0;
               eeAddr = 0;
           }

           DPRINTF(EthernetEEPROM, "EEPROM: opcode: %#X:%d addr: %#X:%d\n",
                   (uint32_t)eeOpcode, (uint32_t) eeOpBits,
                   (uint32_t)eeAddr>>1, (uint32_t)eeAddrBits);
           if (eeOpBits == 8 && !(eeOpcode == EEPROM_READ_OPCODE_SPI ||
                                  eeOpcode == EEPROM_RDSR_OPCODE_SPI ))
               panic("Unknown eeprom opcode: %#X:%d\n", (uint32_t)eeOpcode,
                       (uint32_t)eeOpBits);

       }
       // If driver requests eeprom access, immediately give it to it
       regs.eecd.ee_gnt = regs.eecd.ee_req;
       break;
      case EERD:
       regs.eerd.reg = val;
       break;
      case ICR:
       regs.icd.reg = val;
       break;
      case IMC:
       regs.imc.reg = val;
       break;
      case RCTL:
       regs.rctl.reg = val;
       break;
      case TCTL:
       regs.tctl.reg = val;
       break;
      case PBA:
       regs.pba.rxa = val;
       regs.pba.txa = 64 - regs.pba.rxa;
       break;
      case WUC:
      case LEDCTL:
       ; // We don't care, so don't store anything
       break;
      case MANC:
       regs.manc.reg = val;
       break;
      default:
       if (!(daddr >= VFTA && daddr < (VFTA + VLAN_FILTER_TABLE_SIZE)*4) &&
           !(daddr >= RAL && daddr < (RAL + RCV_ADDRESS_TABLE_SIZE)*4) &&
           !(daddr >= MTA && daddr < (MTA + MULTICAST_TABLE_SIZE)*4))
           panic("Write request to unknown register number: %#x\n", daddr);
    };

    pkt->result = Packet::Success;
    return pioDelay;
}

bool
IGbE::ethRxPkt(EthPacketPtr packet)
{
    panic("Need to implemenet\n");
}

void
IGbE::ethTxDone()
{
    panic("Need to implemenet\n");
}

void
IGbE::serialize(std::ostream &os)
{
    panic("Need to implemenet\n");
}

void
IGbE::unserialize(Checkpoint *cp, const std::string &section)
{
    panic("Need to implemenet\n");
}

BEGIN_DECLARE_SIM_OBJECT_PARAMS(IGbEInt)

    SimObjectParam<EtherInt *> peer;
    SimObjectParam<IGbE *> device;

END_DECLARE_SIM_OBJECT_PARAMS(IGbEInt)

BEGIN_INIT_SIM_OBJECT_PARAMS(IGbEInt)

    INIT_PARAM_DFLT(peer, "peer interface", NULL),
    INIT_PARAM(device, "Ethernet device of this interface")

END_INIT_SIM_OBJECT_PARAMS(IGbEInt)

CREATE_SIM_OBJECT(IGbEInt)
{
    IGbEInt *dev_int = new IGbEInt(getInstanceName(), device);

    EtherInt *p = (EtherInt *)peer;
    if (p) {
        dev_int->setPeer(p);
        p->setPeer(dev_int);
    }

    return dev_int;
}

REGISTER_SIM_OBJECT("IGbEInt", IGbEInt)

BEGIN_DECLARE_SIM_OBJECT_PARAMS(IGbE)

    SimObjectParam<System *> system;
    SimObjectParam<Platform *> platform;
    SimObjectParam<PciConfigData *> configdata;
    Param<uint32_t> pci_bus;
    Param<uint32_t> pci_dev;
    Param<uint32_t> pci_func;
    Param<Tick> pio_latency;
    Param<Tick> config_latency;

END_DECLARE_SIM_OBJECT_PARAMS(IGbE)

BEGIN_INIT_SIM_OBJECT_PARAMS(IGbE)

    INIT_PARAM(system, "System pointer"),
    INIT_PARAM(platform, "Platform pointer"),
    INIT_PARAM(configdata, "PCI Config data"),
    INIT_PARAM(pci_bus, "PCI bus ID"),
    INIT_PARAM(pci_dev, "PCI device number"),
    INIT_PARAM(pci_func, "PCI function code"),
    INIT_PARAM_DFLT(pio_latency, "Programmed IO latency in bus cycles", 1),
    INIT_PARAM(config_latency, "Number of cycles for a config read or write")

END_INIT_SIM_OBJECT_PARAMS(IGbE)

CREATE_SIM_OBJECT(IGbE)
{
    IGbE::Params *params = new IGbE::Params;

    params->name = getInstanceName();
    params->platform = platform;
    params->system = system;
    params->configData = configdata;
    params->busNum = pci_bus;
    params->deviceNum = pci_dev;
    params->functionNum = pci_func;
    params->pio_delay = pio_latency;
    params->config_delay = config_latency;

    return new IGbE(params);
}

REGISTER_SIM_OBJECT("IGbE", IGbE)
<|endoftext|>


<commit_before>//
//  Instruction.hpp
//  Clock Signal
//
//  Created by Thomas Harte on 15/01/21.
//  Copyright © 2021 Thomas Harte. All rights reserved.
//

#ifndef InstructionSets_M50740_Instruction_h
#define InstructionSets_M50740_Instruction_h

#include <cstdint>
#include "../AccessType.hpp"

namespace InstructionSet {
namespace M50740 {

enum class AddressingMode {
	Implied,				Accumulator,			Immediate,
	Absolute,				AbsoluteX,				AbsoluteY,
	ZeroPage,				ZeroPageX,				ZeroPageY,
	XIndirect,				IndirectY,
	Relative,
	AbsoluteIndirect,		ZeroPageIndirect,
	SpecialPage,
	ImmediateZeroPage,
	AccumulatorRelative,	ZeroPageRelative
};

static constexpr auto MaxAddressingMode = int(AddressingMode::ZeroPageRelative);
static constexpr auto MinAddressingMode = int(AddressingMode::Implied);

constexpr int size(AddressingMode mode) {
	// This is coupled to the AddressingMode list above; be careful!
	constexpr int sizes[] = {
		0, 0, 1,
		2, 2, 2,
		1, 1, 1,
		1, 1,
		1,
		2, 1,
		1,
		2,
		1,	2
	};
	static_assert(sizeof(sizes)/sizeof(*sizes) == int(MaxAddressingMode) + 1);
	return sizes[int(mode)];
}

enum class Operation: uint8_t {
	Invalid,

	// Operations that don't access memory.
	BBC0,	BBS0,	BBC1,	BBS1,	BBC2,	BBS2,	BBC3,	BBS3,
	BBC4,	BBS4,	BBC5,	BBS5,	BBC6,	BBS6,	BBC7,	BBS7,
	BCC,	BCS,
	BEQ,	BMI,	BNE,	BPL,
	BVC,	BVS,	BRA,	BRK,
	JMP,	JSR,
	RTI,	RTS,
	CLC,	CLD,	CLI,	CLT,	CLV,
	SEC,	SED,	SEI,	SET,
	INX,	INY,	DEX,	DEY,
	FST,	SLW,
	NOP,
	PHA, 	PHP, 	PLA,	PLP,
	STP,
	TAX,	TAY,	TSX,	TXA,
	TXS,	TYA,

	// Read operations.
	ADC,	SBC,
	AND,	ORA,	EOR,	BIT,
	CMP,	CPX,	CPY,
	LDA,	LDX,	LDY,
	TST,

	// Read-modify-write operations.
	ASL,	LSR,
	CLB0,	SEB0,	CLB1,	SEB1,	CLB2,	SEB2,	CLB3,	SEB3,
	CLB4,	SEB4,	CLB5,	SEB5,	CLB6,	SEB6,	CLB7,	SEB7,
	COM,
	DEC,	INC,
	ROL,	ROR,	RRF,

	// Write operations.
	LDM,
	STA,	STX,	STY,
};

static constexpr auto MaxOperation = int(Operation::STY);
static constexpr auto MinOperation = int(Operation::BBC0);

constexpr AccessType access_type(Operation operation) {
	if(operation < Operation::ADC)	return AccessType::None;
	if(operation < Operation::ASL)	return AccessType::Read;
	if(operation < Operation::LDM)	return AccessType::Write;
	return AccessType::ReadModifyWrite;
}

constexpr bool uses_index_mode(Operation operation) {
	return
		operation == Operation::ADC || operation == Operation::AND ||
		operation == Operation::CMP || operation == Operation::EOR ||
		operation == Operation::LDA || operation == Operation::ORA ||
		operation == Operation::SBC;
}

struct Instruction {
	Operation operation = Operation::Invalid;
	AddressingMode addressing_mode = AddressingMode::Implied;
	uint8_t opcode = 0;

	Instruction(Operation operation, AddressingMode addressing_mode, uint8_t opcode) : operation(operation), addressing_mode(addressing_mode), opcode(opcode) {}
	Instruction(uint8_t opcode) : opcode(opcode) {}
	Instruction() {}
};

}
}

#endif /* InstructionSets_M50740_Instruction_h */
<commit_msg>Corrects bit-selection shifts.<commit_after>//
//  Instruction.hpp
//  Clock Signal
//
//  Created by Thomas Harte on 15/01/21.
//  Copyright © 2021 Thomas Harte. All rights reserved.
//

#ifndef InstructionSets_M50740_Instruction_h
#define InstructionSets_M50740_Instruction_h

#include <cstdint>
#include "../AccessType.hpp"

namespace InstructionSet {
namespace M50740 {

enum class AddressingMode {
	Implied,				Accumulator,			Immediate,
	Absolute,				AbsoluteX,				AbsoluteY,
	ZeroPage,				ZeroPageX,				ZeroPageY,
	XIndirect,				IndirectY,
	Relative,
	AbsoluteIndirect,		ZeroPageIndirect,
	SpecialPage,
	ImmediateZeroPage,
	AccumulatorRelative,	ZeroPageRelative
};

static constexpr auto MaxAddressingMode = int(AddressingMode::ZeroPageRelative);
static constexpr auto MinAddressingMode = int(AddressingMode::Implied);

constexpr int size(AddressingMode mode) {
	// This is coupled to the AddressingMode list above; be careful!
	constexpr int sizes[] = {
		0, 0, 1,
		2, 2, 2,
		1, 1, 1,
		1, 1,
		1,
		2, 1,
		1,
		2,
		1,	2
	};
	static_assert(sizeof(sizes)/sizeof(*sizes) == int(MaxAddressingMode) + 1);
	return sizes[int(mode)];
}

enum class Operation: uint8_t {
	Invalid,

	// Operations that don't access memory.
	BBC0,	BBC1,	BBC2,	BBC3,	BBC4,	BBC5,	BBC6,	BBC7,
	BBS0,	BBS1,	BBS2,	BBS3,	BBS4,	BBS5,	BBS6,	BBS7,
	BCC,	BCS,
	BEQ,	BMI,	BNE,	BPL,
	BVC,	BVS,	BRA,	BRK,
	JMP,	JSR,
	RTI,	RTS,
	CLC,	CLD,	CLI,	CLT,	CLV,
	SEC,	SED,	SEI,	SET,
	INX,	INY,	DEX,	DEY,
	FST,	SLW,
	NOP,
	PHA, 	PHP, 	PLA,	PLP,
	STP,
	TAX,	TAY,	TSX,	TXA,
	TXS,	TYA,

	// Read operations.
	ADC,	SBC,
	AND,	ORA,	EOR,	BIT,
	CMP,	CPX,	CPY,
	LDA,	LDX,	LDY,
	TST,

	// Read-modify-write operations.
	ASL,	LSR,
	CLB0,	CLB1,	CLB2,	CLB3,	CLB4,	CLB5,	CLB6,	CLB7,
	SEB0,	SEB1,	SEB2,	SEB3,	SEB4,	SEB5,	SEB6,	SEB7,
	COM,
	DEC,	INC,
	ROL,	ROR,	RRF,

	// Write operations.
	LDM,
	STA,	STX,	STY,
};

static constexpr auto MaxOperation = int(Operation::STY);
static constexpr auto MinOperation = int(Operation::BBC0);

constexpr AccessType access_type(Operation operation) {
	if(operation < Operation::ADC)	return AccessType::None;
	if(operation < Operation::ASL)	return AccessType::Read;
	if(operation < Operation::LDM)	return AccessType::Write;
	return AccessType::ReadModifyWrite;
}

constexpr bool uses_index_mode(Operation operation) {
	return
		operation == Operation::ADC || operation == Operation::AND ||
		operation == Operation::CMP || operation == Operation::EOR ||
		operation == Operation::LDA || operation == Operation::ORA ||
		operation == Operation::SBC;
}

struct Instruction {
	Operation operation = Operation::Invalid;
	AddressingMode addressing_mode = AddressingMode::Implied;
	uint8_t opcode = 0;

	Instruction(Operation operation, AddressingMode addressing_mode, uint8_t opcode) : operation(operation), addressing_mode(addressing_mode), opcode(opcode) {}
	Instruction(uint8_t opcode) : opcode(opcode) {}
	Instruction() {}
};

}
}

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


<commit_before>/*
 * Copyright (C) 2013-2018 Daniel Nicoletti <dantti12@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
#include "response_p.h"

#include "context_p.h"
#include "engine.h"
#include "enginerequest.h"
#include "common.h"

#include <QtCore/QJsonDocument>

#include <QCryptographicHash>
#include <QEventLoop>

using namespace Cutelyst;

Response::Response(const Headers &defaultHeaders, EngineRequest *engineRequest)
  : d_ptr(new ResponsePrivate(defaultHeaders, engineRequest))
{
    open(QIODevice::WriteOnly);
}

qint64 Response::readData(char *data, qint64 maxlen)
{
    Q_UNUSED(data)
    Q_UNUSED(maxlen)
    return -1;
}

qint64 Response::writeData(const char *data, qint64 len)
{
    Q_D(Response);

    if (len <= 0) {
        return len;
    }

    // Finalize headers if someone manually writes output
    if (!(d->engineRequest->status & EngineRequest::FinalizedHeaders)) {
        if (d->headers.header(QStringLiteral("TRANSFER_ENCODING")) == QLatin1String("chunked")) {
            d->engineRequest->status |= EngineRequest::IOWrite | EngineRequest::Chunked;
        } else {
            // When chunked encoding is not set the client can only know
            // that data is finished if we close the connection
            d->headers.setHeader(QStringLiteral("CONNECTION"), QStringLiteral("close"));
            d->engineRequest->status |= EngineRequest::IOWrite;
        }
        delete d->bodyIODevice;
        d->bodyIODevice = nullptr;
        d->bodyData = QByteArray();

        d->engineRequest->finalizeHeaders();
    }

    return d->engineRequest->write(data, len);
}

Response::~Response()
{
    delete d_ptr->bodyIODevice;
    delete d_ptr;
}

quint16 Response::status() const
{
    Q_D(const Response);
    return d->status;
}

void Response::setStatus(quint16 status)
{
    Q_D(Response);
    d->status = status;
}

bool Response::hasBody() const
{
    Q_D(const Response);
    return !d->bodyData.isEmpty() || d->bodyIODevice || d->engineRequest->status & EngineRequest::IOWrite;
}

QByteArray &Response::body()
{
    Q_D(Response);
    if (d->bodyIODevice) {
        delete d->bodyIODevice;
        d->bodyIODevice = nullptr;
    }

    return d->bodyData;
}

QIODevice *Response::bodyDevice() const
{
    Q_D(const Response);
    return d->bodyIODevice;
}

void Response::setBody(QIODevice *body)
{
    Q_D(Response);
    Q_ASSERT(body && body->isOpen() && body->isReadable());

    if (!(d->engineRequest->status & EngineRequest::IOWrite)) {
        d->bodyData = QByteArray();
        d->bodyIODevice = body;
    }
}

void Response::setBody(const QByteArray &body)
{
    Q_D(Response);
    d->setBodyData(body);
}

void Response::setJsonBody(const QJsonDocument &documment)
{
    Q_D(Response);
    const QByteArray body = documment.toJson(QJsonDocument::Compact);
    d->setBodyData(body);
    d->headers.setContentType(QStringLiteral("application/json"));
}

void Response::setJsonBody(const QString &json)
{
    Q_D(Response);
    d->setBodyData(json.toUtf8());
    d->headers.setContentType(QStringLiteral("application/json"));
}

void Response::setJsonObjectBody(const QJsonObject &object)
{
    Q_D(Response);
    const QByteArray body = QJsonDocument(object).toJson(QJsonDocument::Compact);
    d->setBodyData(body);
    d->headers.setContentType(QStringLiteral("application/json"));
}

void Response::setJsonArrayBody(const QJsonArray &array)
{
    Q_D(Response);
    const QByteArray body = QJsonDocument(array).toJson(QJsonDocument::Compact);
    d->setBodyData(body);
    d->headers.setContentType(QStringLiteral("application/json"));
}

QString Response::contentEncoding() const
{
    Q_D(const Response);
    return d->headers.contentEncoding();
}

void Cutelyst::Response::setContentEncoding(const QString &encoding)
{
    Q_D(Response);
    Q_ASSERT_X(!(d->engineRequest->status & EngineRequest::FinalizedHeaders),
               "setContentEncoding",
               "setting a header value after finalize_headers and the response callback has been called. Not what you want.");

    d->headers.setContentEncoding(encoding);
}

qint64 Response::contentLength() const
{
    Q_D(const Response);
    return d->headers.contentLength();
}

void Response::setContentLength(qint64 length)
{
    Q_D(Response);
    Q_ASSERT_X(!(d->engineRequest->status & EngineRequest::FinalizedHeaders),
               "setContentLength",
               "setting a header value after finalize_headers and the response callback has been called. Not what you want.");

    d->headers.setContentLength(length);
}

QString Response::contentType() const
{
    Q_D(const Response);
    return d->headers.contentType();
}

QString Response::contentTypeCharset() const
{
    Q_D(const Response);
    return d->headers.contentTypeCharset();
}

QVariant Response::cookie(const QByteArray &name) const
{
    Q_D(const Response);
    return QVariant::fromValue(d->cookies.value(name));
}

QList<QNetworkCookie> Response::cookies() const
{
    Q_D(const Response);
    return d->cookies.values();
}

void Response::setCookie(const QNetworkCookie &cookie)
{
    Q_D(Response);
    d->cookies.insert(cookie.name(), cookie);
}

void Response::setCookies(const QList<QNetworkCookie> &cookies)
{
    Q_D(Response);
    for (const QNetworkCookie &cookie : cookies) {
        d->cookies.insert(cookie.name(), cookie);
    }
}

int Response::removeCookies(const QByteArray &name)
{
    Q_D(Response);
    return d->cookies.remove(name);
}

void Response::redirect(const QUrl &url, quint16 status)
{
    Q_D(Response);
    d->location = url;
    d->status = status;

    if (url.isValid()) {
        const QString location = QString::fromLatin1(url.toEncoded(QUrl::FullyEncoded));
        qCDebug(CUTELYST_RESPONSE) << "Redirecting to" << location << status;

        d->headers.setHeader(QStringLiteral("LOCATION"), location);
        d->headers.setContentType(QStringLiteral("text/html; charset=utf-8"));

        const QString buf = QStringLiteral(
                    "<!DOCTYPE html PUBLIC \"-//W3C//DTD XHTML 1.0"
                    "Strict//EN\" \"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd\">\n"
                    "\n"
                    "  \n"
                    "    <title>Moved</title>\n"
                    "  \n"
                    "  \n"
                    "
This item has moved here.
\n"
                               "  \n"
                               "\n");
        setBody(buf);
    } else {
        d->headers.removeHeader(QStringLiteral("LOCATION"));
        qCDebug(CUTELYST_ENGINE) << "Invalid redirect removing header" << url << status;
    }
}

void Response::redirect(const QString &url, quint16 status)
{
    redirect(QUrl(url), status);
}

QUrl Response::location() const
{
    Q_D(const Response);
    return d->location;
}

QString Response::header(const QString &field) const
{
    Q_D(const Response);
    return d->headers.header(field);
}

void Response::setHeader(const QString &field, const QString &value)
{
    Q_D(Response);
    Q_ASSERT_X(!(d->engineRequest->status & EngineRequest::FinalizedHeaders),
               "setHeader",
               "setting a header value after finalize_headers and the response callback has been called. Not what you want.");

    d->headers.setHeader(field, value);
}

Headers &Response::headers()
{
    Q_D(Response);
    return d->headers;
}

bool Response::isSequential() const
{
    return true;
}

qint64 Response::size() const
{
    Q_D(const Response);
    if (d->engineRequest->status & EngineRequest::IOWrite) {
        return -1;
    } else if (d->bodyIODevice) {
        return d->bodyIODevice->size();
    } else {
        return d->bodyData.size();
    }
}

bool Response::webSocketHandshake(const QString &key, const QString &origin, const QString &protocol)
{
    Q_D(Response);
    return d->engineRequest->webSocketHandshake(key, origin, protocol);
}

bool Response::webSocketTextMessage(const QString &message)
{
    Q_D(Response);
    return d->engineRequest->webSocketSendTextMessage(message);
}

bool Response::webSocketBinaryMessage(const QByteArray &message)
{
    Q_D(Response);
    return d->engineRequest->webSocketSendBinaryMessage(message);
}

bool Response::webSocketPing(const QByteArray &payload)
{
    Q_D(Response);
    return d->engineRequest->webSocketSendPing(payload);
}

bool Response::webSocketClose(quint16 code, const QString &reason)
{
    Q_D(Response);
    return d->engineRequest->webSocketClose(code, reason);
}

void ResponsePrivate::setBodyData(const QByteArray &body)
{
    if (!(engineRequest->status & EngineRequest::IOWrite)) {
        if (bodyIODevice) {
            delete bodyIODevice;
            bodyIODevice = nullptr;
        }
        bodyData = body;
        headers.setContentLength(body.size());
    }
}

#include "moc_response.cpp"
<commit_msg>core: Fix not deleting QIODevice body when another body is set<commit_after>/*
 * Copyright (C) 2013-2018 Daniel Nicoletti <dantti12@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
#include "response_p.h"

#include "context_p.h"
#include "engine.h"
#include "enginerequest.h"
#include "common.h"

#include <QtCore/QJsonDocument>

#include <QCryptographicHash>
#include <QEventLoop>

using namespace Cutelyst;

Response::Response(const Headers &defaultHeaders, EngineRequest *engineRequest)
  : d_ptr(new ResponsePrivate(defaultHeaders, engineRequest))
{
    open(QIODevice::WriteOnly);
}

qint64 Response::readData(char *data, qint64 maxlen)
{
    Q_UNUSED(data)
    Q_UNUSED(maxlen)
    return -1;
}

qint64 Response::writeData(const char *data, qint64 len)
{
    Q_D(Response);

    if (len <= 0) {
        return len;
    }

    // Finalize headers if someone manually writes output
    if (!(d->engineRequest->status & EngineRequest::FinalizedHeaders)) {
        if (d->headers.header(QStringLiteral("TRANSFER_ENCODING")) == QLatin1String("chunked")) {
            d->engineRequest->status |= EngineRequest::IOWrite | EngineRequest::Chunked;
        } else {
            // When chunked encoding is not set the client can only know
            // that data is finished if we close the connection
            d->headers.setHeader(QStringLiteral("CONNECTION"), QStringLiteral("close"));
            d->engineRequest->status |= EngineRequest::IOWrite;
        }
        delete d->bodyIODevice;
        d->bodyIODevice = nullptr;
        d->bodyData = QByteArray();

        d->engineRequest->finalizeHeaders();
    }

    return d->engineRequest->write(data, len);
}

Response::~Response()
{
    delete d_ptr->bodyIODevice;
    delete d_ptr;
}

quint16 Response::status() const
{
    Q_D(const Response);
    return d->status;
}

void Response::setStatus(quint16 status)
{
    Q_D(Response);
    d->status = status;
}

bool Response::hasBody() const
{
    Q_D(const Response);
    return !d->bodyData.isEmpty() || d->bodyIODevice || d->engineRequest->status & EngineRequest::IOWrite;
}

QByteArray &Response::body()
{
    Q_D(Response);
    if (d->bodyIODevice) {
        delete d->bodyIODevice;
        d->bodyIODevice = nullptr;
    }

    return d->bodyData;
}

QIODevice *Response::bodyDevice() const
{
    Q_D(const Response);
    return d->bodyIODevice;
}

void Response::setBody(QIODevice *body)
{
    Q_D(Response);
    Q_ASSERT(body && body->isOpen() && body->isReadable());

    if (!(d->engineRequest->status & EngineRequest::IOWrite)) {
        d->bodyData = QByteArray();
        if (d->bodyIODevice) {
            delete d->bodyIODevice;
        }
        d->bodyIODevice = body;
    }
}

void Response::setBody(const QByteArray &body)
{
    Q_D(Response);
    d->setBodyData(body);
}

void Response::setJsonBody(const QJsonDocument &documment)
{
    Q_D(Response);
    const QByteArray body = documment.toJson(QJsonDocument::Compact);
    d->setBodyData(body);
    d->headers.setContentType(QStringLiteral("application/json"));
}

void Response::setJsonBody(const QString &json)
{
    Q_D(Response);
    d->setBodyData(json.toUtf8());
    d->headers.setContentType(QStringLiteral("application/json"));
}

void Response::setJsonObjectBody(const QJsonObject &object)
{
    Q_D(Response);
    const QByteArray body = QJsonDocument(object).toJson(QJsonDocument::Compact);
    d->setBodyData(body);
    d->headers.setContentType(QStringLiteral("application/json"));
}

void Response::setJsonArrayBody(const QJsonArray &array)
{
    Q_D(Response);
    const QByteArray body = QJsonDocument(array).toJson(QJsonDocument::Compact);
    d->setBodyData(body);
    d->headers.setContentType(QStringLiteral("application/json"));
}

QString Response::contentEncoding() const
{
    Q_D(const Response);
    return d->headers.contentEncoding();
}

void Cutelyst::Response::setContentEncoding(const QString &encoding)
{
    Q_D(Response);
    Q_ASSERT_X(!(d->engineRequest->status & EngineRequest::FinalizedHeaders),
               "setContentEncoding",
               "setting a header value after finalize_headers and the response callback has been called. Not what you want.");

    d->headers.setContentEncoding(encoding);
}

qint64 Response::contentLength() const
{
    Q_D(const Response);
    return d->headers.contentLength();
}

void Response::setContentLength(qint64 length)
{
    Q_D(Response);
    Q_ASSERT_X(!(d->engineRequest->status & EngineRequest::FinalizedHeaders),
               "setContentLength",
               "setting a header value after finalize_headers and the response callback has been called. Not what you want.");

    d->headers.setContentLength(length);
}

QString Response::contentType() const
{
    Q_D(const Response);
    return d->headers.contentType();
}

QString Response::contentTypeCharset() const
{
    Q_D(const Response);
    return d->headers.contentTypeCharset();
}

QVariant Response::cookie(const QByteArray &name) const
{
    Q_D(const Response);
    return QVariant::fromValue(d->cookies.value(name));
}

QList<QNetworkCookie> Response::cookies() const
{
    Q_D(const Response);
    return d->cookies.values();
}

void Response::setCookie(const QNetworkCookie &cookie)
{
    Q_D(Response);
    d->cookies.insert(cookie.name(), cookie);
}

void Response::setCookies(const QList<QNetworkCookie> &cookies)
{
    Q_D(Response);
    for (const QNetworkCookie &cookie : cookies) {
        d->cookies.insert(cookie.name(), cookie);
    }
}

int Response::removeCookies(const QByteArray &name)
{
    Q_D(Response);
    return d->cookies.remove(name);
}

void Response::redirect(const QUrl &url, quint16 status)
{
    Q_D(Response);
    d->location = url;
    d->status = status;

    if (url.isValid()) {
        const QString location = QString::fromLatin1(url.toEncoded(QUrl::FullyEncoded));
        qCDebug(CUTELYST_RESPONSE) << "Redirecting to" << location << status;

        d->headers.setHeader(QStringLiteral("LOCATION"), location);
        d->headers.setContentType(QStringLiteral("text/html; charset=utf-8"));

        const QString buf = QStringLiteral(
                    "<!DOCTYPE html PUBLIC \"-//W3C//DTD XHTML 1.0"
                    "Strict//EN\" \"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd\">\n"
                    "\n"
                    "  \n"
                    "    <title>Moved</title>\n"
                    "  \n"
                    "  \n"
                    "
This item has moved here.
\n"
                               "  \n"
                               "\n");
        setBody(buf);
    } else {
        d->headers.removeHeader(QStringLiteral("LOCATION"));
        qCDebug(CUTELYST_ENGINE) << "Invalid redirect removing header" << url << status;
    }
}

void Response::redirect(const QString &url, quint16 status)
{
    redirect(QUrl(url), status);
}

QUrl Response::location() const
{
    Q_D(const Response);
    return d->location;
}

QString Response::header(const QString &field) const
{
    Q_D(const Response);
    return d->headers.header(field);
}

void Response::setHeader(const QString &field, const QString &value)
{
    Q_D(Response);
    Q_ASSERT_X(!(d->engineRequest->status & EngineRequest::FinalizedHeaders),
               "setHeader",
               "setting a header value after finalize_headers and the response callback has been called. Not what you want.");

    d->headers.setHeader(field, value);
}

Headers &Response::headers()
{
    Q_D(Response);
    return d->headers;
}

bool Response::isSequential() const
{
    return true;
}

qint64 Response::size() const
{
    Q_D(const Response);
    if (d->engineRequest->status & EngineRequest::IOWrite) {
        return -1;
    } else if (d->bodyIODevice) {
        return d->bodyIODevice->size();
    } else {
        return d->bodyData.size();
    }
}

bool Response::webSocketHandshake(const QString &key, const QString &origin, const QString &protocol)
{
    Q_D(Response);
    return d->engineRequest->webSocketHandshake(key, origin, protocol);
}

bool Response::webSocketTextMessage(const QString &message)
{
    Q_D(Response);
    return d->engineRequest->webSocketSendTextMessage(message);
}

bool Response::webSocketBinaryMessage(const QByteArray &message)
{
    Q_D(Response);
    return d->engineRequest->webSocketSendBinaryMessage(message);
}

bool Response::webSocketPing(const QByteArray &payload)
{
    Q_D(Response);
    return d->engineRequest->webSocketSendPing(payload);
}

bool Response::webSocketClose(quint16 code, const QString &reason)
{
    Q_D(Response);
    return d->engineRequest->webSocketClose(code, reason);
}

void ResponsePrivate::setBodyData(const QByteArray &body)
{
    if (!(engineRequest->status & EngineRequest::IOWrite)) {
        if (bodyIODevice) {
            delete bodyIODevice;
            bodyIODevice = nullptr;
        }
        bodyData = body;
        headers.setContentLength(body.size());
    }
}

#include "moc_response.cpp"
<|endoftext|>


<commit_before><commit_msg>Ensure correct initialization of vision info at BeginPlay.<commit_after><|endoftext|>


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

 QGroundControl Open Source Ground Control Station

 (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

 This file is part of the QGROUNDCONTROL project

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

 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

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

/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "APMFirmwarePlugin.h"
#include "Generic/GenericFirmwarePlugin.h"
#include "QGCMAVLink.h"

#include <QDebug>

IMPLEMENT_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin)
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")

static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)");
static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)");
static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)");

// Regex to parse version text coming from APM, gives out firmware type, major, minor and patch level numbers
static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|ArduCopter|ArduPlane|ArduRover) +[vV](\\d*)\\.*(\\d*)*\\.*(\\d*)*");

// minimum firmware versions that don't suffer from mavlink severity inversion bug.
// https://github.com/diydrones/apm_planner/issues/788
static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0");
static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0");
static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");

/*
 * @brief APMFirmwareVersion is a small class to represent the firmware version
 * It encabsules vehicleType, major version, minor version and patch level version
 * and provides accessors for the same.
 * isValid() can be used, to know whether version infromation is available or not
 * supports < operator
 */
APMFirmwareVersion::APMFirmwareVersion(const QString &versionText)
{
    _major         = 0;
    _minor         = 0;
    _patch         = 0;

    _parseVersion(versionText);
}

bool APMFirmwareVersion::isValid() const
{
    return !_versionString.isEmpty();
}

bool APMFirmwareVersion::isBeta() const
{
    return _versionString.contains(QStringLiteral(".rc"));
}

bool APMFirmwareVersion::isDev() const
{
    return _versionString.contains(QStringLiteral(".dev"));
}

bool APMFirmwareVersion::operator <(const APMFirmwareVersion& other) const
{
    int myVersion = _major << 16 | _minor << 8 | _patch ;
    int otherVersion = other.majorNumber() << 16 | other.minorNumber() << 8 | other.patchNumber();
    return myVersion < otherVersion;
}

void APMFirmwareVersion::_parseVersion(const QString &versionText)
{
    if (versionText.isEmpty()) {
        return;
    }

    if (VERSION_REXP.indexIn(versionText) == -1) {
        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
                                        << "version text to be parsed" << versionText;
        return;
    }

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

    if (capturedTexts.count() < 5) {
        qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything"
                                        << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts();
        return;
    }

    // successful extraction of version numbers
    // even though we could have collected the version string atleast
    // but if the parsing has faild, not much point
    _versionString = versionText;
    _vehicleType   = capturedTexts[1];
    _major         = capturedTexts[2].toInt();
    _minor         = capturedTexts[3].toInt();
    _patch         = capturedTexts[4].toInt();
}

APMFirmwarePlugin::APMFirmwarePlugin(QObject* parent) :
    FirmwarePlugin(parent)
{
     _textSeverityAdjustmentNeeded = false;
}

bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
    Q_UNUSED(capabilities);

    // FIXME: No capabilitis yet supported

    return false;
}

QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);

    return QList<VehicleComponent*>();
}

QStringList APMFirmwarePlugin::flightModes(void)
{
    // FIXME: NYI

    qWarning() << "APMFirmwarePlugin::flightModes not supported";

    return QStringList();
}

QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
{
    // FIXME: Nothing more than generic support yet
    return GenericFirmwarePlugin::instance()->flightMode(base_mode, custom_mode);
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
    Q_UNUSED(flightMode);
    Q_UNUSED(base_mode);
    Q_UNUSED(custom_mode);

    qWarning() << "APMFirmwarePlugin::setFlightMode called on base class, not supported";

    return false;
}

int APMFirmwarePlugin::manualControlReservedButtonCount(void)
{
    // We don't know whether the firmware is going to used any of these buttons.
    // So reserve them all.
    return -1;
}

void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
{
    if (message->msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
        mavlink_param_value_t paramValue;
        mavlink_param_union_t paramUnion;

        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
        // type they are. Fix that up to correct usage.

        mavlink_msg_param_value_decode(message, &paramValue);

        switch (paramValue.param_type) {
            case MAV_PARAM_TYPE_UINT8:
                paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT8:
                paramUnion.param_int8 = (int8_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_UINT16:
                paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT16:
                paramUnion.param_int16 = (int16_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT32:
                paramUnion.param_int32 = (int32_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_REAL32:
                paramUnion.param_float = paramValue.param_value;
                break;
            default:
                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
        }

        paramValue.param_value = paramUnion.param_float;

        mavlink_msg_param_value_encode(message->sysid, message->compid, message, &paramValue);

    } else if (message->msgid == MAVLINK_MSG_ID_PARAM_SET) {
        mavlink_param_set_t     paramSet;
        mavlink_param_union_t   paramUnion;

        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
        // type they are. Fix it back to the wrong way on the way out.

        mavlink_msg_param_set_decode(message, &paramSet);

        paramUnion.param_float = paramSet.param_value;

        switch (paramSet.param_type) {
            case MAV_PARAM_TYPE_UINT8:
                paramSet.param_value = paramUnion.param_uint8;
                break;
            case MAV_PARAM_TYPE_INT8:
                paramSet.param_value = paramUnion.param_int8;
                break;
            case MAV_PARAM_TYPE_UINT16:
                paramSet.param_value = paramUnion.param_uint16;
                break;
            case MAV_PARAM_TYPE_INT16:
                paramSet.param_value = paramUnion.param_int16;
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramSet.param_value = paramUnion.param_uint32;
                break;
            case MAV_PARAM_TYPE_INT32:
                paramSet.param_value = paramUnion.param_int32;
                break;
            case MAV_PARAM_TYPE_REAL32:
                // Already in param_float
                break;
            default:
                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
        }

        mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
    }

    if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT)
    {
        QByteArray b;
        b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
        mavlink_msg_statustext_get_text(message, b.data());
        // Ensure NUL-termination
        b[b.length()-1] = '\0';
        QString text = QString(b);
        qCDebug(APMFirmwarePluginLog) << text;

        // if don't know firmwareVersion yet, try and see this message contains it
        if (!_firmwareVersion.isValid()) {
            if (text.contains(APM_COPTER_REXP) || text.contains(APM_PLANE_REXP) || text.contains(APM_ROVER_REXP)) {
                // found version string
                _firmwareVersion = APMFirmwareVersion(text);
                _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion);
            }
        }

        // adjust mesasge if needed
        if (_textSeverityAdjustmentNeeded) {
            _adjustSeverity(message);
        }
    }
}

bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
{
   if (!firmwareVersion.isValid()) {
       return false;
   }

   bool adjustmentNeeded = false;
   if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   }

   return adjustmentNeeded;
}

void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
        case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
            statusText.severity = MAV_SEVERITY_WARNING;
            break;
        case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
            statusText.severity = MAV_SEVERITY_ALERT;
            break;
        case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
            statusText.severity = MAV_SEVERITY_CRITICAL;
            break;
    }

    mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
}

void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
    // Streams are not started automatically on APM stack
    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,        2);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS,    2);
    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,        2);
    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,           3);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,             10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,             10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,             3);
}
<commit_msg>fixed more review comments<commit_after>/*=====================================================================

 QGroundControl Open Source Ground Control Station

 (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

 This file is part of the QGROUNDCONTROL project

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

 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

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

/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "APMFirmwarePlugin.h"
#include "Generic/GenericFirmwarePlugin.h"
#include "QGCMAVLink.h"

#include <QDebug>

IMPLEMENT_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin)
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")

static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)");
static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)");
static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)");

// Regex to parse version text coming from APM, gives out firmware type, major, minor and patch level numbers
static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|ArduCopter|ArduPlane|ArduRover) +[vV](\\d*)\\.*(\\d*)*\\.*(\\d*)*");

// minimum firmware versions that don't suffer from mavlink severity inversion bug.
// https://github.com/diydrones/apm_planner/issues/788
static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0");
static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0");
static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");

/*
 * @brief APMFirmwareVersion is a small class to represent the firmware version
 * It encabsules vehicleType, major version, minor version and patch level version
 * and provides accessors for the same.
 * isValid() can be used, to know whether version infromation is available or not
 * supports < operator
 */
APMFirmwareVersion::APMFirmwareVersion(const QString &versionText)
{
    _major         = 0;
    _minor         = 0;
    _patch         = 0;

    _parseVersion(versionText);
}

bool APMFirmwareVersion::isValid() const
{
    return !_versionString.isEmpty();
}

bool APMFirmwareVersion::isBeta() const
{
    return _versionString.contains(QStringLiteral(".rc"));
}

bool APMFirmwareVersion::isDev() const
{
    return _versionString.contains(QStringLiteral(".dev"));
}

bool APMFirmwareVersion::operator <(const APMFirmwareVersion& other) const
{
    int myVersion = _major << 16 | _minor << 8 | _patch ;
    int otherVersion = other.majorNumber() << 16 | other.minorNumber() << 8 | other.patchNumber();
    return myVersion < otherVersion;
}

void APMFirmwareVersion::_parseVersion(const QString &versionText)
{
    if (versionText.isEmpty()) {
        return;
    }

    if (VERSION_REXP.indexIn(versionText) == -1) {
        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
                                        << "version text to be parsed" << versionText;
        return;
    }

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

    if (capturedTexts.count() < 5) {
        qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything"
                                        << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts();
        return;
    }

    // successful extraction of version numbers
    // even though we could have collected the version string atleast
    // but if the parsing has faild, not much point
    _versionString = versionText;
    _vehicleType   = capturedTexts[1];
    _major         = capturedTexts[2].toInt();
    _minor         = capturedTexts[3].toInt();
    _patch         = capturedTexts[4].toInt();
}

APMFirmwarePlugin::APMFirmwarePlugin(QObject* parent) :
    FirmwarePlugin(parent)
{
     _textSeverityAdjustmentNeeded = false;
}

bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
    Q_UNUSED(capabilities);

    // FIXME: No capabilitis yet supported

    return false;
}

QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);

    return QList<VehicleComponent*>();
}

QStringList APMFirmwarePlugin::flightModes(void)
{
    // FIXME: NYI

    qWarning() << "APMFirmwarePlugin::flightModes not supported";

    return QStringList();
}

QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
{
    // FIXME: Nothing more than generic support yet
    return GenericFirmwarePlugin::instance()->flightMode(base_mode, custom_mode);
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
    Q_UNUSED(flightMode);
    Q_UNUSED(base_mode);
    Q_UNUSED(custom_mode);

    qWarning() << "APMFirmwarePlugin::setFlightMode called on base class, not supported";

    return false;
}

int APMFirmwarePlugin::manualControlReservedButtonCount(void)
{
    // We don't know whether the firmware is going to used any of these buttons.
    // So reserve them all.
    return -1;
}

void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
{
    if (message->msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
        mavlink_param_value_t paramValue;
        mavlink_param_union_t paramUnion;

        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
        // type they are. Fix that up to correct usage.

        mavlink_msg_param_value_decode(message, &paramValue);

        switch (paramValue.param_type) {
            case MAV_PARAM_TYPE_UINT8:
                paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT8:
                paramUnion.param_int8 = (int8_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_UINT16:
                paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT16:
                paramUnion.param_int16 = (int16_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT32:
                paramUnion.param_int32 = (int32_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_REAL32:
                paramUnion.param_float = paramValue.param_value;
                break;
            default:
                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
        }

        paramValue.param_value = paramUnion.param_float;

        mavlink_msg_param_value_encode(message->sysid, message->compid, message, &paramValue);

    } else if (message->msgid == MAVLINK_MSG_ID_PARAM_SET) {
        mavlink_param_set_t     paramSet;
        mavlink_param_union_t   paramUnion;

        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
        // type they are. Fix it back to the wrong way on the way out.

        mavlink_msg_param_set_decode(message, &paramSet);

        paramUnion.param_float = paramSet.param_value;

        switch (paramSet.param_type) {
            case MAV_PARAM_TYPE_UINT8:
                paramSet.param_value = paramUnion.param_uint8;
                break;
            case MAV_PARAM_TYPE_INT8:
                paramSet.param_value = paramUnion.param_int8;
                break;
            case MAV_PARAM_TYPE_UINT16:
                paramSet.param_value = paramUnion.param_uint16;
                break;
            case MAV_PARAM_TYPE_INT16:
                paramSet.param_value = paramUnion.param_int16;
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramSet.param_value = paramUnion.param_uint32;
                break;
            case MAV_PARAM_TYPE_INT32:
                paramSet.param_value = paramUnion.param_int32;
                break;
            case MAV_PARAM_TYPE_REAL32:
                // Already in param_float
                break;
            default:
                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
        }

        mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
    }

    if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT)
    {
        if (!_firmwareVersion.isValid()) {
            QByteArray b;
            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
            mavlink_msg_statustext_get_text(message, b.data());
            // Ensure NUL-termination
            b[b.length()-1] = '\0';
            QString text = QString(b);
            qCDebug(APMFirmwarePluginLog) << text;

            // if don't know firmwareVersion yet, try and see this message contains it
            if (text.contains(APM_COPTER_REXP) || text.contains(APM_PLANE_REXP) || text.contains(APM_ROVER_REXP)) {
                // found version string
                _firmwareVersion = APMFirmwareVersion(text);
                _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion);
            }
        }

        // adjust mesasge if needed
        if (_textSeverityAdjustmentNeeded) {
            _adjustSeverity(message);
        }
    }
}

bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
{
   if (!firmwareVersion.isValid()) {
       return false;
   }

   bool adjustmentNeeded = false;
   if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   }

   return adjustmentNeeded;
}

void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
        case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
            statusText.severity = MAV_SEVERITY_WARNING;
            break;
        case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
            statusText.severity = MAV_SEVERITY_ALERT;
            break;
        case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
            statusText.severity = MAV_SEVERITY_CRITICAL;
            break;
    }

    mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
}

void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
    // Streams are not started automatically on APM stack
    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,        2);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS,    2);
    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,        2);
    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,           3);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,             10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,             10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,             3);
}
<|endoftext|>


<commit_before><commit_msg>Include <string><commit_after><|endoftext|>