예제 #1
0
void radioTransceiverTask(const void *arg) {

	unsigned long tv;
	static unsigned long  radio_last_tv2 = 0;
	while (true){

#if 1
#if 0 //debug
		tv=millis();
		 Serial.println(tv-radio_last_tv2);
		 radio_last_tv2=tv;
#endif

			memset(transceiverBuffer, '\0', sizeof(transceiverBuffer));
			sprintf(transceiverBuffer,
						"@%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%d:%d:%d:%d:%d#",
						getRoll(), getPitch(), getYaw(), getPidSp(&rollAttitudePidSettings),
						getPidSp(&pitchAttitudePidSettings),  getYawCenterPoint() + getPidSp(&yawAttitudePidSettings),
						getRollGyro(), getPitchGyro(), getYawGyro(),
						getThrottlePowerLevel(), getMotorPowerLevelCCW1(),
						getMotorPowerLevelCW1(), getMotorPowerLevelCCW2(),
						getMotorPowerLevelCW2());
	
				Udp.beginPacket(broadcastIP, localPort);
				Udp.write(transceiverBuffer,strlen(transceiverBuffer));
				Udp.endPacket();
				
			increasePacketAccCounter();
	    os_thread_yield();
		delay(TRANSMIT_TIMER);
#endif
		}

}
예제 #2
0
/**
 * update attitude
 *
 * @param 
 * 		void
 *
 * @return
 *		void
 *
 */
void attitudeUpdate(){

	float yrpAttitude[3];
	float pryRate[3];
	float xyzAcc[3];
	float xComponent[3];
	float yComponent[3];
	float zComponent[3];
	float xyzMagnet[3];
#if CHECK_ATTITUDE_UPDATE_LOOP_TIME
	struct timeval tv_c;
	static struct timeval tv_l;
	unsigned long timeDiff=0;
	
	gettimeofday(&tv_c,NULL);
	timeDiff=GET_USEC_TIMEDIFF(tv_c,tv_l);
	_DEBUG(DEBUG_NORMAL,"attitude update duration=%ld us\n",timeDiff);
	UPDATE_LAST_TIME(tv_c,tv_l);
#endif	
			
	getYawPitchRollInfo(yrpAttitude, pryRate, xyzAcc, xComponent, yComponent, zComponent,xyzMagnet); 

	setYaw(yrpAttitude[0]);
	setRoll(yrpAttitude[1]);
	setPitch(yrpAttitude[2]);
	setYawGyro(-pryRate[2]);
	setPitchGyro(pryRate[0]);
	setRollGyro(-pryRate[1]);
	setXAcc(xyzAcc[0]);
	setYAcc(xyzAcc[1]);
	setZAcc(xyzAcc[2]);
	setXGravity(zComponent[0]);
	setYGravity(zComponent[1]);
	setZGravity(zComponent[2]);
	setVerticalAcceleration(deadband((getXAcc() * zComponent[0] + getYAcc() * zComponent[1]
		+ getZAcc() * zComponent[2] - 1.f) * 100.f,3.f) );
	setXAcceleration(deadband((getXAcc() * xComponent[0] + getYAcc() * xComponent[1]
		+ getZAcc() * xComponent[2]) * 100.f,3.f) );
	setYAcceleration(deadband((getXAcc() * yComponent[0] + getYAcc() * yComponent[1]
		+ getZAcc() * yComponent[2]) * 100.f,3.f) );

	_DEBUG(DEBUG_ATTITUDE,
			"(%s-%d) ATT: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n", __func__,
			__LINE__, getRoll(), getPitch(), getYaw());
	_DEBUG(DEBUG_GYRO,
			"(%s-%d) GYRO: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n",
			__func__, __LINE__, getRollGyro(), getPitchGyro(),
			getYawGyro());
	_DEBUG(DEBUG_ACC, "(%s-%d) ACC: x=%3.3f y=%3.3f z=%3.3f\n",
			__func__, __LINE__, getXAcc(), getYAcc(), getZAcc());
	
}
예제 #3
0
/**
 * get the output of angular velocity PID controler
 *
 * @param rollRateOutput
 * 		output of roll angular velocity PID controler
 *
 * @param pitchRateOutput
 * 		output of pitch angular velocity PID controler
 *
 * @param yawRateOutput
 * 		output of yaw angular velocity PID controler
 */
void getRatePidOutput(float *rollRateOutput, float *pitchRateOutput,
		float *yawRateOutput) {

	setPidSp(&rollRatePidSettings, rollAttitudeOutput);
	setPidSp(&pitchRatePidSettings, pitchAttitudeOutput);
	setPidSp(&yawRatePidSettings, yawAttitudeOutput);
	*rollRateOutput = pidCalculation(&rollRatePidSettings, getRollGyro(), true,
	true, true);
	*pitchRateOutput = pidCalculation(&pitchRatePidSettings, getPitchGyro(),
	true, true, true);
	*yawRateOutput = pidCalculation(&yawRatePidSettings, getYawGyro(), true,
	true, true);

	_DEBUG(DEBUG_RATE_PID_OUTPUT,
			"(%s-%d) rate pid output: roll=%.5f, pitch=%.5f, yaw=%.5f\n",
			__func__, __LINE__, *rollRateOutput, *pitchRateOutput,
			*yawRateOutput);
}