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
}
}
/**
* 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());
}
/**
* 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);
}
