예제 #1
0
int turn(float heading){
	float currentHeading = getHeading(fd);
	float deltaHeading = wrap180(heading - currentHeading); // deltaheading: negative means turn left, positive means turn right


	const int tolerance = 5;
	printf("deltaHeading: %f, currentHeading: %f, tolerance: %d\n", deltaHeading, currentHeading, tolerance);
	//while(abs((int) deltaHeading) > tolerance && keepGoing){
	while(keepGoing && abs(deltaHeading) > tolerance){
		if(deltaHeading > 0){ //turn right
			printf("**turning right**deltaHeading positive: %f, currentHeading: %f\n", deltaHeading, currentHeading);
			gpio_set_value(LEFT_FWD_GPIO, 1);
			gpio_set_value(LEFT_BCK_GPIO, 0);
			gpio_set_value(RIGHT_BCK_GPIO, 1);
			gpio_set_value(RIGHT_FWD_GPIO, 0); 		
		} else { // turn left
			printf("**turning left**deltaHeading negative: %f, currentHeading: %f\n", deltaHeading, currentHeading);
			gpio_set_value(LEFT_BCK_GPIO, 1);
			gpio_set_value(LEFT_FWD_GPIO, 0);
			gpio_set_value(RIGHT_FWD_GPIO, 1);
			gpio_set_value(RIGHT_BCK_GPIO, 0);			
		}
		currentHeading = getHeading(fd);
		deltaHeading = wrap180(heading - currentHeading);
	}

	gpio_set_value(LEFT_FWD_GPIO, 0);
	gpio_set_value(LEFT_BCK_GPIO, 0);
	gpio_set_value(RIGHT_FWD_GPIO, 0);
	gpio_set_value(RIGHT_BCK_GPIO, 0);
}
예제 #2
0
파일: Hero.cpp 프로젝트: nriviere/Pathogen
void Hero::shoot()
{
	if (munitions[currentMunition] > 0)
	{
		Projectile *projectile = NULL;
		switch (currentMunition){
		case 0: projectile = new Neutrophile(engine);
			projectile->setHeading(getHeading());
			break;
		case 1: projectile = shootMonocyte();
			
			break;
		case 2: projectile = new Lymphocyte(engine, lymphocyteTag);
			projectile->setHeading(getHeading());
			break;
		default: return;
			break;
		}
		if (projectile != NULL)
		{
			engine->addObject(projectile);
			munitions[currentMunition]--;
			engine->getParentEngine()->getSoundEngine()->playSound(SoundEngine::TEST_SOUND_ID);
		}
	}
}
예제 #3
0
void HMC5883L::calibrate()
{
    int                     i = 0;
    float                   nSumMinX = 0.0f;
    float                   nSumMaxX = 0.0f;
    float                   nSumMinY = 0.0f;
    float                   nSumMaxY = 0.0f;
    const int               nLoopCnt = 10;
    
    for(i=0 ; i<5 ; i++)
    {
        int16_t             x, y, z;

        getHeading(&x, &y, &z);
        
        delay(20);
    }
    
    for(i=0 ; i<nLoopCnt ; i++)
    {
        float               nMinX = 0.0f;
        float               nMaxX = 0.0f;
        float               nMinY = 0.0f;
        float               nMaxY = 0.0f;
        int16_t             x, y, z;
        
        getHeading(&x, &y, &z);
        
        // Determine Min / Max values
        if(x < nMinX)
            nMinX = x;
        if(x > nMaxX)
            nMaxX = x;
        if(y < nMinY)
            nMinY = y;
        if(y > nMaxY)
            nMaxY = y;
        
        nSumMinX += nMinX;
        nSumMaxX += nMaxX;
        nSumMinY += nMinY;
        nSumMaxY += nMaxY;
        
        delay(20);
    }

    // Calculate offsets
    magoffset[0] = (int16_t)((nSumMaxX + nSumMinX) / 2 / nLoopCnt);
    magoffset[1] = (int16_t)((nSumMaxY + nSumMinY) / 2 / nLoopCnt);
}
예제 #4
0
파일: vec3.cpp 프로젝트: Charence/stk-code
/** Sets the pitch and the roll of this vector to follow the normal given. The
 *  heading is taken from this vector.
 *  \param normal The normal vector to which pitch and roll should be aligned.
 */
void Vec3::setPitchRoll(const Vec3 &normal)
{
    const float X = sin(getHeading());
    const float Z = cos(getHeading());
    // Compute the angle between the normal of the plane and the line to
    // (x,0,z).  (x,0,z) is normalised, so are the coordinates of the plane,
    // which simplifies the computation of the scalar product.
    float pitch = ( normal.getX()*X + normal.getZ()*Z );  // use ( x,0,z)
    float roll  = (-normal.getX()*Z + normal.getZ()*X );  // use (-z,0,x)

    // The actual angle computed above is between the normal and the (x,y,0)
    // line, so to compute the actual angles 90 degrees must be subtracted.
    setPitch(-acosf(pitch) + NINETY_DEGREE_RAD);
    setRoll (-acosf(roll)  + NINETY_DEGREE_RAD);
}   // setPitchRoll
예제 #5
0
  inline void FileParse::getHead(std::ifstream& fin) {
    if (fin.eof()) return;
    // Create a new head node
    HeadNode *node = new HeadNode;

    // Look for the heading. Ends with ":", or "{". If "{" is encountered, body = true.
    bool body = false;
    getHeading(fin, node->heading, body);

    // Message
    message += (tabs() + "Level " + toStr(level) + ": Heading: [" + node->heading + "]\n");

    // Set node as the current head
    node->parent = currentHead;
    currentHead = node;

    // Look for parameters
    bool newLine = passSpaces(fin);
    
    // Get the parameters - adds them to the current head node
    if (!fin.eof() && !newLine && !body)
      body = getParameters(fin);

    ++level;
    if (body && !fin.eof()) getBody(fin);
    --level;

    // Add this node to the parent node
    node->parent->subHeads.push_back(node);

    // Return to parent node
    currentHead = node->parent;
  }
예제 #6
0
void AISteering::turnTo(const Position &destination)
{
	float uncorrectedAngleDifference = getAngleDifference(destination);
	float correctedAngleDifference   = uncorrectedAngleDifference;

	//angle corrections
	while(correctedAngleDifference <= -M_PI) 
		correctedAngleDifference += 2*M_PI;
	while(correctedAngleDifference >= M_PI) 
		correctedAngleDifference -= 2*M_PI;
	
	//check the uncorrected angle to see if you're facing the target
	//if not, then steer accordingly
	if( abs(correctedAngleDifference) < getDt()*bot->type->getTurnSpeed() )
	{
		botPosition->setR(getHeading(*botPosition, destination));
		bot->turnLeft(false);
		bot->turnRight(false);
	}
/*	if(isFacing(destination))
	{
		bot->turnLeft(false);
		bot->turnRight(false);	
	}*/
	else if(correctedAngleDifference > 0)
	{
		bot->turnLeft(false);
		bot->turnRight(true);
	}
	else
	{
		bot->turnLeft(true);
		bot->turnRight(false);
	}
}
예제 #7
0
파일: ai.cpp 프로젝트: Corosauce/AI_TestBed
bool ai_agent::LOSCheck(long x1, long y1, long x2, long y2) {
	//Must supply tile coordinates
	if (x1 == x2) {
		int aaaaaa = 0;
	}

	float heading = getHeading(x1*ts+(ts/2),y1*ts+(ts/2),x2*ts+(ts/2),y2*ts+(ts/2));
	float distance = getDistance(x1*ts+(ts/2),y1*ts+(ts/2),x2*ts+(ts/2),y2*ts+(ts/2));

	//heading += 0.00000001f;

	

	if (distance <= ts) {
		return true;
	}

	for (float tempDist = 0; tempDist < distance; tempDist += ts/4) {
		//long tile = getRelX(physPtr1,heading,tempDist);
		long xx2 = (x1*ts)+(ts/2);
		long yy2 = (y1*ts)+(ts/2);
		int xx1 = (int)(this->ptrWorld->ptrPhysics->getRelX(xx2,heading,tempDist)/ts);
		int yy1 = (int)(this->ptrWorld->ptrPhysics->getRelY(yy2,heading,tempDist)/ts);
		BLKSTR* blockTemp = this->ptrWorld->Map->MapSource->MapGetBlock(xx1,yy1);
		if (blockTemp->tl) {
			return false;
		}
	}
	return true;
}
예제 #8
0
파일: ai.cpp 프로젝트: Corosauce/AI_TestBed
bool ai_agent::LOSCheck(nodeInfo* node1, nodeInfo* node2) {
	//float ts = ptrWorld->Map->WorldInfo.tile_size;
	
	float heading = getHeading(node1->x*ts+(ts/2),node1->y*ts+(ts/2),node2->x*ts+(ts/2),node2->y*ts+(ts/2));
	float distance = getDistance(node1->x*ts+(ts/2),node1->y*ts+(ts/2),node2->x*ts+(ts/2),node2->y*ts+(ts/2));

	//heading += 0.00000001f;

	if (distance <= ts) {
		return true;
	}

	for (float tempDist = 0; tempDist < distance; tempDist += ts/4) {
		//long tile = getRelX(physPtr1,heading,tempDist);
		long x2 = (node1->x*ts)+(ts/2);
		long y2 = (node1->y*ts)+(ts/2);
		int x1 = (int)(this->ptrWorld->ptrPhysics->getRelX(x2,heading,tempDist)/ts);
		int y1 = (int)(this->ptrWorld->ptrPhysics->getRelY(y2,heading,tempDist)/ts);
		BLKSTR* blockTemp = this->ptrWorld->Map->MapSource->MapGetBlock(x1,y1);
		if (blockTemp->tl) {
			return false;
		}
	}
	return true;
}
예제 #9
0
//------------------------------------------------------------------------------
// atReleaseInit() -- Init weapon data at release
//------------------------------------------------------------------------------
void Missile::atReleaseInit()
{
   // First the base class will setup the initial conditions
   BaseClass::atReleaseInit();

   if (getDynamicsModel() == 0) {
      // set initial commands
      cmdPitch = (LCreal) getPitch();
      cmdHeading = (LCreal) getHeading();
      cmdVelocity = vpMax;

      if (getTargetTrack() != 0) {
         // Set initial range and range dot
         osg::Vec3 los = getTargetTrack()->getPosition();
         trng = los.length();
         trngT = trng;
      }
      else if (getTargetPlayer() != 0) {
         // Set initial range and range dot
         osg::Vec3 los = getTargetPosition();
         trng = los.length();
         trngT = trng;
      }
      else {
         trng = 0;
      }

      // Range dot
      trdot = 0;
      trdotT = 0;
   }
}
예제 #10
0
//returns a vector with 9 doubles representing the sensory
//input to the network..
boost::dynamic_bitset<> * GTPWrapper::sqlook(int * pos, bool first)
{
  //int * pos = getPos(first);
  //  cout << "in eye: pos0(height): " << pos[0] << " pos1: " << pos[1] << endl;
  boost::dynamic_bitset<> * ret = new boost::dynamic_bitset<>((int)pow(eyesize,2)*2);
//   cout << "creating addr: " << ret << endl;
  //  ret.insert(ret.begin(),);
  int ah = 0, aw = 0;
  int d = (int)floor(eyesize/2);
//   cout << "======= sqlook start ========= " << endl;
  for(int i=0;i<eyesize;i++){
    for(int i2=0;i2<eyesize;i2++){
      ah = pos[0]+i-d-1;
      aw = pos[1]+i2-d-1;
      
      if( ah<0 || aw<0 || ah==bsize || aw==bsize)//(((bsize-(ah+1))*bsize)+(2*aw))<0 || (unsigned int)(((bsize-(ah+1))*bsize)+(aw*2)+1)>=board.size() || ((2*aw)+1)>=(bsize*2) )
	{
//  	  cout << "adding ovalue: i:"<<i<<" i2:"<<i2<<" pos0: " 
//  	       << pos[0] << " pos1: " << pos[1] 
//  	       << " ah: " << ah << " aw: " << aw 
//  	       << " bsize: " << bsize << " the calc to coord " << (i*eyesize*2)+(i2*2) << "," << (i*eyesize*2)+(i2*2)+1 << endl;

	  (*ret)[(i*eyesize*2)+(i2*2)] = 1;
	  (*ret)[(i*eyesize*2)+(i2*2)+1] = 1;
// 	  cout << "ret now: " << *ret<<endl;
	} 
      else 
	{
// 	  cout << "adding goodvalue: i:"<<i<<" i2:"<<i2<<" pos0: " 
//  	       << pos[0] << " pos1: " << pos[1] 
//  	       << " ah: " << ah << " aw: " << aw << " the calc: " << ((bsize-(ah+1))*bsize)+(aw*2) << "," << ((bsize-(ah+1))*bsize)+(aw*2)+1
// 	       << " value: " << board[((bsize-(ah+1))*bsize)+(aw*2)] << "," << board[((bsize-(ah+1))*bsize)+(aw*2)+1] 
// 	       << " at coords: " << (i*2*eyesize)+(i2*2) << "," << (i*2*eyesize)+(i2*2)+1 << endl;
//    	  cout << "adding value ("<<ah<<","
//    	       <<aw<<"): " << board.at(((bsize-(ah+1))*bsize)+aw) << endl;
	  (*ret)[(i*eyesize*2)+(i2*2)] = board[((bsize-(ah+1))*bsize*2)+(aw*2)];
	  (*ret)[(i*eyesize*2)+(i2*2)+1] = board[((bsize-(ah+1))*bsize*2)+(aw*2)+1];
// 	  if(board[((bsize-(ah+1))*bsize)+(aw*2)]||board[((bsize-(ah+1))*bsize)+(aw*2)+1])
// 	    cout << "one of them over 0 printing: " << ret << endl;
	}
    }
  }
  int * heading = getHeading(first);
//         cout << "before:" <<endl << printvector(ret,eyesize);
  //  if(headingsupport){
  if(*heading!=0){
    boost::dynamic_bitset<> * tmp = ret;
    if(*heading==1)
      ret = twistit(ret,*heading,eyesize);
    else if(*heading==2)
      ret = twistit(ret,*heading,eyesize);
    else if(*heading==3)
      ret = twistit(ret,*heading,eyesize);
    delete tmp;
  }
//   else
//     cout << "not twisting.. " << endl;
//   cout << "returning addr: " << ret << " print: " << *ret << endl;
  return ret;
}
예제 #11
0
파일: ai.cpp 프로젝트: Corosauce/AI_TestBed
void ai_agent::UpdateTarget() {
	//this->ptrWorld->los_checks++;
	if (EntMemory->targetList.size() > 0) {

		Ent_Target = EntMemory->targetList.front();

		if (Ent_Target->ID > -2) {
			if ((Ent_Target->phys_state == -1) || (Ent->ID == Ent_Target->ID)) {
				AttackStop();
				EntMemory->targetList.pop_front();
			} else {
				//Ent->target.x = EntMemory->ptrTarget->position.x;
				//Ent->target.y = EntMemory->ptrTarget->position.y;
				Ent->target.x = Ent_Target->position.x;
				Ent->target.y = Ent_Target->position.y;
				Ent->target.distance = getDistance(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
				Ent->target.angle = getHeading(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
			}
		} else {
			EntMemory->targetList.pop_front();
			
		}
	} else {
		
		if (EntMemory->hasTargetToGuard) {
			
			Ent_Target = EntMemory->targetGuard;

			if (Ent_Target->ID > -2) {
				if ((Ent_Target->phys_state == -1) || (Ent->ID == Ent_Target->ID)) {		
					AttackStop();
					//EntMemory->targetList.pop_front();
				} else {
					//Ent->target.x = EntMemory->ptrTarget->position.x;
					//Ent->target.y = EntMemory->ptrTarget->position.y;
					Ent->target.x = Ent_Target->position.x;
					Ent->target.y = Ent_Target->position.y;
					Ent->target.distance = getDistance(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
					Ent->target.angle = getHeading(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
				}
			}
		}
	}


}
예제 #12
0
int newHeading(int a){ //(+) rep to the right, (-) rep to the left
    int new_heading = getHeading()+a;
    if(new_heading<0)
        new_heading+=360;
    else if(new_heading>360)
        new_heading-=360;
    return new_heading;
}
예제 #13
0
bool CFootballPlayer::isBallKickable() const
{
    btVector3 ballPos = m_simulationManager->getBallPosition();
    btVector3 toBall = ballPos - getPosition();
    btScalar dot = getHeading().dot(toBall.normalized());
    bool canKick = false;
    if(dot > 0 && (getPosition().distance(ballPos) < 2)) {
        canKick = true;
    }
    return canKick;
}
예제 #14
0
int main(int argc, char** argv) {
    initTruck();
    int start_time = timeInSecs();
    long double start_lat = getLatitude();
    long double start_lon = getLongitude();
    int start_heading = getHeading();
    int phase = 0;

    setSteering(0);
    setThrottle(50);
    while(TRUE){

        int target_heading;
        if(phase == 0){
            if(distance(start_lat, getLatitude(), start_lon, getLongitude())>5){
                setThrottle(30);
                setSteering(60);
                phase = 1;
                target_heading = newHeading(90);
                }
        }
        else if (phase == 1){
            if(getHeading()==target_heading){
                setSteering(-60);
                target_heading=start_heading;
                phase = 2;

            }
        }
        else if (phase == 2){
            if(getHeading()==target_heading){
                setSteering(0);
                setThrottle(0);
                while(TRUE);
            }
        }

        background();
    }
    return (EXIT_SUCCESS);
}
예제 #15
0
const NAString
ElemDDLColViewDef::displayLabel2() const
{
  if (isHeadingSpecified())
  {
    return NAString("Heading:   ") + getHeading();
  }
  else
  {
    return NAString("Heading not spec.");
  }
}
/**
	this method is used to return the current heading of the character, specified as an angle measured in radians
*/
double Character::getHeadingAngle(){
	//first we need to get the current heading of the character. Also, note that q and -q represent the same rotation
	Quaternion q = getHeading();
	if (q.s<0){
		q.s = -q.s;
		q.v = -q.v;
	}
	double currentHeading = 2 * safeACOS(q.s);
	if (q.v.dotProductWith(PhysicsGlobals::up) < 0)
		currentHeading = -currentHeading;
	return currentHeading;
}
예제 #17
0
파일: rover.c 프로젝트: km1003/RaspberryPi
void testCompass()
{
  start = getuSecs();
  float heading = getHeading();
  end = getuSecs();
  if(heading > maxF) maxF = heading;
  if(heading < minF || minF == 0) minF = heading;
  dif = (int)(end-start);
  if(dif > maxt) maxt = dif;
  if(dif < mint || mint == 0) mint = dif;
  printf("heading: %3.2f,\tmin:%3.2f,\tmax:%3.2f\n", heading, minF, maxF);
  printf("elapsed: %4dus, \tmin:%dus,\tmax:%dus\n", dif, mint, maxt);
}
AREXPORT void ArActionDesired::log(void) const
{
  // all those maxes and movement parameters
  if (getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "\tMaxTransVel %.0f", getMaxVel());
  if (getMaxNegVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "\tMaxTransNegVel %.0f", 
	       getMaxNegVel());
  if (getTransAccelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "\tTransAccel %.0f", getTransAccel());
  if (getTransDecelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "\tTransDecel %.0f", getTransDecel());
  if (getMaxRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%25s\tMaxRotVel %.0f", "",
	       getMaxRotVel());
  if (getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%25s\tRotAccel %.0f", "",
	       getRotAccel());
  if (getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%25s\tRotDecel %.0f", "",
	       getRotDecel());

  if (getMaxLeftLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%12s\tMaxLeftLatVel %.0f", "",
	       getMaxLeftLatVel());
  if (getMaxRightLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%12s\tMaxRightLatVel %.0f", "",
	       getMaxRightLatVel());
  if (getLatAccelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%12s\tLatAccel %.0f", "",
	       getLatAccel());
  if (getLatDecelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%12s\tLatDecel %.0f", "",
	       getLatDecel());
  
  // the actual movement part
  if (getVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "\tVel %.0f", getVel());
  if (getHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%25s\tHeading %.0f", "", 
	       getHeading());
  if (getDeltaHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%25s\tDeltaHeading %.0f", "", 
	       getDeltaHeading());
  if (getRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%25s\tRotVel %.0f", "", 
	       getRotVel());
  if (getLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
    ArLog::log(ArLog::Normal, "%12s\tLatVel %.0f", "", 
	       getLatVel());
}
예제 #19
0
파일: ai.cpp 프로젝트: Corosauce/AI_TestBed
bool ai_agent::LOSCheck(phys_obj* physPtr1, phys_obj* physPtr2) {
	//this->ptrWorld->los_checks++;
	float heading = getHeading(physPtr1->position.x,physPtr1->position.y,physPtr2->position.x,physPtr2->position.y);
	float distance = getDistance(physPtr1->position.x,physPtr1->position.y,physPtr2->position.x,physPtr2->position.y);

	for (float tempDist = 0; tempDist < distance; tempDist += ptrWorld->Map->WorldInfo.tile_size/2) {
		//long tile = getRelX(physPtr1,heading,tempDist);
		BLKSTR* blockTemp = this->ptrWorld->Map->MapSource->MapGetBlock(this->ptrWorld->ptrPhysics->getRelX(physPtr1->position.x,heading,tempDist)/ptrWorld->Map->WorldInfo.tile_size,this->ptrWorld->ptrPhysics->getRelY(physPtr1->position.y,heading,tempDist)/ptrWorld->Map->WorldInfo.tile_size);
		if (blockTemp->tl) {
			return false;
		}
	}
	return true;
}
예제 #20
0
/*
 * send calculated heading to uart3
 */
void sendHeading(void) {
	short b;
	char accl[20];
	accl[19] = '\0';
	b = usprintf(accl, "$HEAD,%i*\n", (int) getHeading()); //$HEAD,value*
#ifndef wlan
	nrf24l01p_send(accl);
#else
	for (int i = 0; i < b && accl[i] != '\0'; i++) {
		ROM_UARTCharPut(UART3_BASE, accl[i]);
	}

#endif
}
예제 #21
0
float HM55B::getStableHeading(int sample_size){
	float sum = 0;
	int neg_count = 0;
	float heading = 0;
	for (int i = 0; i < sample_size; i++){
		heading = getHeading();
		if (heading < 0){ 
			neg_count++;
		}
		sum = sum + fabs(heading);
	}
	if (neg_count < (sample_size / 2)){
		return sum / sample_size;
	} else {
		return -1 * sum / sample_size;
	}
}
예제 #22
0
/**
 * \par Function
 *   getAngle
 * \par Description
 *   Calculate the yaw angle by the calibrated sensor value.
 * \param[in]
 *   None
 * \par Output
 *   None
 * \return
 *   The angle value from 0 to 360 degrees. If not success, return an error code.
 * \par Others
 *   Will return a correct angle when you keep the MeCompass working in the plane which have calibrated.
 */
double MeCompass::getAngle(void)
{
  int16_t cx,cy,cz;
  double compass_angle;
  int8_t return_value = 0;
  deviceCalibration();  
  return_value = getHeading(&cx, &cy, &cz);
  if(return_value != 0)
  {
    return (double)return_value;
  }  
  if(Calibration_Flag == true)
  {
    cx = (cx + Cal_parameter.X_excursion) * Cal_parameter.X_gain;
    cy = (cy + Cal_parameter.Y_excursion) * Cal_parameter.Y_gain;
    cz = (cz + Cal_parameter.Z_excursion) * Cal_parameter.Z_gain;  

    if(Cal_parameter.Rotation_Axis == 1)  //X_Axis
    {
      compass_angle = atan2( (double)cy, (double)cz );
    }
    else if(Cal_parameter.Rotation_Axis == 2)  //Y_Axis
    {
      compass_angle = atan2( (double)cx, (double)cz );
    }
    else if(Cal_parameter.Rotation_Axis == 3)  //Z_Axis
    {
      compass_angle = atan2( (double)cy, (double)cx );
    }
  }
  else
  {
    compass_angle = atan2( (double)cy, (double)cx );
  }  

  if(compass_angle < 0)
  {
    compass_angle = (compass_angle + 2 * COMPASS_PI) * 180 / COMPASS_PI;
  }
  else
  {
    compass_angle = compass_angle * 180 / COMPASS_PI;
  }
  return compass_angle;
}
int writeDatalink(float frequency){

    if (time - lastTime > frequency) {
        lastTime = time;

        struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();

        if (statusData == 0){
            return 0;
        }
        
        statusData->lat = getLatitude();
        statusData->lon = getLongitude();
        statusData->millis = getUTCTime();
        statusData->groundSpeed = getSpeed();
        statusData->heading = getHeading();
        statusData->lastCommandSent = lastCommandSentCode;
        statusData->errorCodes = getErrorCodes();
        statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
        statusData->steeringSetpoint = getPWM(1);
        statusData->throttleSetpoint = getPWM(2);
        statusData->steeringOutput = sData;
        statusData->throttleOutput = tData;
        statusData->debugChar = debugChar;
        statusData->debugInt = debugInt;
        statusData->debugFloat = debugFloat;


        if (BLOCKING_MODE) {
            sendTelemetryBlock(statusData);
            destroyTelemetryBlock(statusData);
        } else {
            return pushOutboundTelemetryQueue(statusData);
        }
    }
    else if (time < lastTime){
        lastTime = time;
        return 0;
    }
    return 0;
}
예제 #24
0
파일: HMC5883L.cpp 프로젝트: e4deen/DOSv1.0
void HMC5883L::Calibration(int count)
{
	//평균 계산을 위한 변수
	float average_offset = 0;
	int16_t tmp_x = 0, tmp_y = 0, tmp_z = 0;

	//현재 offset 초기화
	Heading_offset = 0;

	for(int i = 0; i < count; i++)
	{
		float tmp_yaw;
		getHeading(&tmp_x, &tmp_y, &tmp_z);
		getYaw(tmp_x, tmp_y, &tmp_yaw);
		if(tmp_yaw < 0) tmp_yaw *= -1;

		average_offset += tmp_yaw;
	}

	//새로운 offset 적용
	Heading_offset = average_offset / count;
}
예제 #25
0
task CalcHeading()
{
    prevTime =currTime = nPgmTime;

    while(true)
    {

      eraseDisplay();
      getHeading() ;
      nxtDisplayTextLine(6, "heading %1.4f", heading);

      if(heading > -5)
        gPwr = 20;
      else if(heading < 5)
        gPwr = -20;
      else
      {
        gPwr = 0;
      }

      wait1Msec(10);
    }
}
//
// Vector getRelationship(target)
// Last modified: 03Sep2006
//
// Returns the relationship from this robot
// to the parameterized target vector.
//
// Returns:     the relationship from this robot to the target vector
// Parameters:
//      target  in/out  the target vector being related to
//
Vector Robot::getRelationship(Vector &target) const
{
    Vector temp = target - *this;
    temp.rotateRelative(-getHeading());
    return temp;
}   // getRelationship(Vector &) const
//
// void translateRelative(v)
// Last modified: 03Sep2006
//
// Translates the robot relative to itself based
// on the parameterized translation vector.
//
// Returns:     <none>
// Parameters:
//      v       in      the translation vector
//
void Robot::translateRelative(Vector v)
{
    v.rotateRelative(getHeading());
    x += v.x;
    y += v.y;
}   // rotateRelative(const Vector)
예제 #28
0
float getCompassHeading(void){
	return getHeading(fd);
}
예제 #29
0
파일: Spherical.cpp 프로젝트: RobR89/KayLib
void Spherical::setLocation(double lat, double lon) {
  set(lat, lon, getHeading());
}
예제 #30
0
// Function called when timer expires
void SysTick_Handler(void)
{
	int currentHeading = 0;
	currentHeading = getHeading();
	printf("Heading: %d\n", currentHeading);
}