Example #1
0
void GPS_NewData(void) {
	static uint32_t nav_loopTimer;
	uint32_t dist;
	int32_t  dir;
	uint8_t axis;
  int16_t speed;
	uint16_t c = GPSBufferAvailable();
	while (c--) {
		if (GPS_newFrame(GPSBufferRead())) {
			if (GPS_Info.GPS_update == 1) 
				GPS_Info.GPS_update = 0;
			else 
				GPS_Info.GPS_update = 1;
			
			if (fgps.GPS_FIX && GPS_Info.GPS_numSat >= 5) {
				if (!checkArm()) {
					fgps.GPS_FIX_HOME = 0;
				}
				if (!fgps.GPS_FIX_HOME && checkArm()) {
					GPS_reset_home_position();
				}
				//Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
				GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
				for (axis = 0; axis< 2; axis++) {
					GPS_data[axis] = GPS_Info.GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
					GPS_degree[axis] = GPS_data[axis] / 10000000;  // get the degree to assure the sum fits to the int32_t
					// How close we are to a degree line ? its the first three digits from the fractions of degree
					// later we use it to Check if we are close to a degree line, if yes, disable averaging,
					fraction3[axis] = (GPS_data[axis]- GPS_degree[axis]*10000000) / 10000;
					GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
					GPS_filter[axis][GPS_filter_index] = GPS_data[axis] - (GPS_degree[axis]*10000000); 
					GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
					GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
					if ( nav_mode == NAV_MODE_POSHOLD) {      //we use gps averaging only in poshold mode...
						if ( fraction3[axis]>1 && fraction3[axis]<999 ) 
							GPS_Info.GPS_coord[axis] = GPS_filtered[axis];
					}
				}
#endif
				//Time for calculating x,y speed and navigation pids
				dTnav = (float)(getTickCount() - nav_loopTimer)/ 10000.0f;
				nav_loopTimer = getTickCount();
				// prevent runup from bad GPS
				dTnav = min(dTnav, 1.0f);  
				//calculate distance and bearings for gui and other stuff continously - From home to copter
				GPS_distance_cm_bearing(&GPS_Info.GPS_coord[LAT],&GPS_Info.GPS_coord[LON],&GPS_Info.GPS_home[LAT],&GPS_Info.GPS_home[LON],&dist,&dir);
				GPS_Info.GPS_distanceToHome = dist/100;
				GPS_Info.GPS_directionToHome = dir/100;
				
				if (!fgps.GPS_FIX_HOME) {     //If we don't have home set, do not display anything
					GPS_Info.GPS_distanceToHome = 0;
					GPS_Info.GPS_directionToHome = 0;
				}
        //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
        GPS_calc_velocity();        
        if (fgps.GPS_HOLD_MODE || fgps.GPS_HOME_MODE){    //ok we are navigating 
          //do gps nav calculations here, these are common for nav and poshold  
          #if defined(GPS_LEAD_FILTER)
          GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
          GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
          #else
          GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
          GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
          #endif
          switch (nav_mode) {
            case NAV_MODE_POSHOLD: 
              //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
              GPS_calc_poshold();
              break;
            case NAV_MODE_WP:
              speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV);      //slow navigation 
              // use error as the desired rate towards the target
              //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
              GPS_calc_nav_rate(speed);
              //Tail control
              if (NAV_CONTROLS_HEADING) {
                if (NAV_TAIL_FIRST) {
                  magHold = wrap_18000(nav_bearing-18000)/100;
                } else {
                  magHold = nav_bearing/100;
                }
              }
              // Are we there yet ?(within 2 meters of the destination)
              if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){         //if yes switch to poshold mode
                nav_mode = NAV_MODE_POSHOLD;
                if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
              } 
              break;               
          }
        } //end of gps calcs  
      }
    }
  }
}
void ArmContactModelProvider::update(ArmContactModel& model)
{
  MODIFY("module:ArmContactModelProvider:parameters", p);
  DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
  DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
  DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
  DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
  DECLARE_PLOT("module:ArmContactModelProvider:errorDurationLeft");
  DECLARE_PLOT("module:ArmContactModelProvider:errorDurationRight");
  DECLARE_PLOT("module:ArmContactModelProvider:errorYThreshold");
  DECLARE_PLOT("module:ArmContactModelProvider:errorXThreshold");
  DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
  DECLARE_PLOT("module:ArmContactModelProvider:contactRight");

  DECLARE_DEBUG_DRAWING("module:ArmContactModelProvider:armContact", "drawingOnField");

  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 400, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 600, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 800, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1000, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);


  /* Buffer arm angles */
  struct ArmAngles angles;
  angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
  angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
  angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
  angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
  angleBuffer.add(angles);
  
  /* Reset in case of a fall or penalty */
  if(theFallDownState.state == FallDownState::onGround || theRobotInfo.penalty != PENALTY_NONE)
  {
    leftErrorBuffer.init();
    rightErrorBuffer.init();
  }

  Pose2D odometryOffset = theOdometryData - lastOdometry;
  lastOdometry = theOdometryData;
  
  const Vector3<>& leftHandPos3D = theRobotModel.limbs[MassCalibration::foreArmLeft].translation;
  Vector2<> leftHandPos(leftHandPos3D.x, leftHandPos3D.y);
  Vector2<> leftHandSpeed = (odometryOffset + Pose2D(leftHandPos) - Pose2D(lastLeftHandPos)).translation / theFrameInfo.cycleTime;
  float leftFactor = std::max(0.f, 1.f - leftHandSpeed.abs() / p.speedBasedErrorReduction);
  lastLeftHandPos = leftHandPos;

  const Vector3<>& rightHandPos3D = theRobotModel.limbs[MassCalibration::foreArmRight].translation;
  Vector2<> rightHandPos(rightHandPos3D.x, rightHandPos3D.y);
  Vector2<> rightHandSpeed = (odometryOffset + Pose2D(rightHandPos) - Pose2D(lastRightHandPos)).translation / theFrameInfo.cycleTime;
  float rightFactor = std::max(0.f, 1.f - rightHandSpeed.abs() / p.speedBasedErrorReduction);
  lastRightHandPos = rightHandPos;
  
  /* Check for arm contact */
  // motion types to take into account: stand, walk (if the robot is upright)
  if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
     (theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering) &&
     (theGameInfo.state == STATE_PLAYING || theGameInfo.state == STATE_READY) &&
     (theRobotInfo.penalty == PENALTY_NONE)) // TICKET 897: ArmContact only if robot is not penalized
  {
    checkArm(LEFT, leftFactor);
    checkArm(RIGHT, rightFactor);

    //left and right are projections of the 3 dimensional shoulder-joint vector
    //onto the x-y plane.
    Vector2f left = leftErrorBuffer.getAverageFloat();
    Vector2f right = rightErrorBuffer.getAverageFloat();

    
    //Determine if we are being pushed or not
    bool leftX  = fabs(left.x) > fromDegrees(p.errorXThreshold);
    bool leftY  = fabs(left.y) > fromDegrees(p.errorYThreshold);
    bool rightX = fabs(right.x)> fromDegrees(p.errorXThreshold);
    bool rightY = fabs(right.y)> fromDegrees(p.errorYThreshold);

    // update the model
    model.contactLeft  = leftX || leftY;
    model.contactRight = rightX || rightY;

    // The duration of the contact is counted upwards as long as the error
    //remains. Otherwise it is reseted to 0.
    model.durationLeft  = model.contactLeft  ? model.durationLeft + 1 : 0;
    model.durationRight = model.contactRight ? model.durationRight + 1 : 0;

    model.contactLeft &= model.durationLeft < p.malfunctionThreshold;
    model.contactRight &= model.durationRight < p.malfunctionThreshold;


    if(model.contactLeft)
    {
      model.timeOfLastContactLeft = theFrameInfo.time;
    }
    if(model.contactRight)
    {
      model.timeOfLastContactRight = theFrameInfo.time;
    }
    
    model.pushDirectionLeft = getDirection(LEFT, leftX, leftY, left);
    model.pushDirectionRight = getDirection(RIGHT, rightX, rightY, right);

    model.lastPushDirectionLeft = model.pushDirectionLeft != ArmContactModel::NONE ? model.pushDirectionLeft : model.lastPushDirectionLeft;
    model.lastPushDirectionRight = model.pushDirectionRight != ArmContactModel::NONE ? model.pushDirectionRight : model.lastPushDirectionRight;

    PLOT("module:ArmContactModelProvider:errorLeftX",         toDegrees(left.x));
    PLOT("module:ArmContactModelProvider:errorRightX",        toDegrees(right.x));
    PLOT("module:ArmContactModelProvider:errorLeftY",         toDegrees(left.y));
    PLOT("module:ArmContactModelProvider:errorRightY",        toDegrees(right.y));
    PLOT("module:ArmContactModelProvider:errorDurationLeft",  model.durationLeft);
    PLOT("module:ArmContactModelProvider:errorDurationRight", model.durationRight);
    PLOT("module:ArmContactModelProvider:errorYThreshold",    toDegrees(p.errorYThreshold));
    PLOT("module:ArmContactModelProvider:errorXThreshold",    toDegrees(p.errorXThreshold));
    PLOT("module:ArmContactModelProvider:contactLeft",        model.contactLeft ? 10.0 : 0.0);
    PLOT("module:ArmContactModelProvider:contactRight",       model.contactRight ? 10.0 : 0.0);

    ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(left.y) * SCALE), toDegrees(left.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::green);
    ARROW("module:ArmContactModelProvider:armContact", 0, 0, toDegrees(right.y) * SCALE, toDegrees(right.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::red);

    COMPLEX_DRAWING("module:ArmContactModelProvider:armContact",
    {
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1300, 20, ColorClasses::black, "LEFT");
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(left.x));
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 900, 20, ColorClasses::black,  "ErrorY: " << toDegrees(left.y));
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 500, 20, ColorClasses::black,  ArmContactModel::getName(model.pushDirectionLeft));
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 300, 20, ColorClasses::black,  "Time: " << model.timeOfLastContactLeft);

      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1300, 20, ColorClasses::black, "RIGHT");
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(right.x));
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 900, 20, ColorClasses::black,  "ErrorY: " << toDegrees(right.y));
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 500, 20, ColorClasses::black,  ArmContactModel::getName(model.pushDirectionRight));
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 300, 20, ColorClasses::black,  "Time: " << model.timeOfLastContactRight);

      if (model.contactLeft)
      {
        CROSS("module:ArmContactModelProvider:armContact", -2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
      }
      if (model.contactRight)
      {
        CROSS("module:ArmContactModelProvider:armContact", 2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
      }
  });
void ArmContactModelProvider::update(ArmContactModel& ArmContactModel,
                                     const JointRequest& theJointRequest,
                                     const FallDownState& theFallDownState,
                                     const MotionInfo& theMotionInfo,
                                     const FilteredJointData& theFilteredJointData,
                                     const FrameInfo& theFrameInfo)
{
  //MODIFY("module:ArmContactModelProvider:parameters", p);

    /*
  DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
  DECLARE_PLOT("module:ArmContactModelProvider:contactRight");
  PLOT("module:ArmContactModelProvider:contactLeft", ArmContactModel.contactLeft ? 7.5 : 2.5);
  PLOT("module:ArmContactModelProvider:contactRight", ArmContactModel.contactRight ? 7.5 : 2.5);

  DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
  DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
  DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
  DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
  PLOT("module:ArmContactModelProvider:errorLeftX", errorLeftX);
  PLOT("module:ArmContactModelProvider:errorRightX", errorRightX);
  PLOT("module:ArmContactModelProvider:errorLeftY", errorLeftY);
  PLOT("module:ArmContactModelProvider:errorRightY", errorRightY);
*/
  /* Buffer arm angles */
  struct ArmAngles angles;
  angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
  angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
  angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
  angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
  angleBuffer.add(angles);

  /* Decrease errors */
  errorLeftX = max(0.0f, errorLeftX - p.errorXDecrease);
  errorLeftY = max(0.0f, errorLeftY - p.errorYDecrease);
  errorRightX = max(0.0f, errorRightX - p.errorXDecrease);
  errorRightY = max(0.0f, errorRightY - p.errorYDecrease);

  /* Reset in case of a fall */
  if(theFallDownState.state == FallDownState::onGround)
  {
    errorLeftX = 0.0f;
    errorLeftY = 0.0f;
    errorRightX = 0.0f;
    errorRightY = 0.0f;
  }

  /* Check for arm contact */
  // motion types to take into account: stand, walk (if the robot is upright)
  if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
     (theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering))
  {
    bool left = checkArm(true, theFilteredJointData);
    bool right = checkArm(false, theFilteredJointData);
    if(p.debugMode && theFrameInfo.getTimeSince(lastSoundTime) > soundDelay &&
       ((left && !ArmContactModel.contactLeft) || (right && !ArmContactModel.contactRight)))
    {
      lastSoundTime = theFrameInfo.time;
      //SoundPlayer::play("arm.wav");
    }
    ArmContactModel.contactLeft = left;
    ArmContactModel.contactRight = right;
  }
  else
  {
    ArmContactModel.contactLeft = false;
    ArmContactModel.contactRight = false;
  }
}