/**
* init a simple moving average filter entity
*
* @param smaStruct
* a SMA filter entity
*
* @param name
* name of this entity
*
* @param movAgeSize
* size of window of SMA
*
* @return
* void
*
*/
void initSmaFilterEntity(SMA_STRUCT *smaStruct,char *name, int movAgeSize){
strcpy(smaStruct->name, name);
memset(smaStruct->buf,0.f,SMA_BUFER_SIZE);
smaStruct->movAgeSize=LIMIT_MIN_MAX_VALUE(movAgeSize,0,SMA_BUFER_SIZE);
smaStruct->curIndex=0;
}
/**
* get the output of attitude PID controler, this output will become a input for angular velocity PID controler
*
* @param
* void
*
* @return
* value
*
*/
void getAttitudePidOutput() {
rollAttitudeOutput = LIMIT_MIN_MAX_VALUE(
pidCalculation(&rollAttitudePidSettings, getRoll(),true,true,true),
-getGyroLimit(), getGyroLimit());
pitchAttitudeOutput =
LIMIT_MIN_MAX_VALUE(
pidCalculation(&pitchAttitudePidSettings, getPitch(),true,true,true),
-getGyroLimit(), getGyroLimit());
yawAttitudeOutput =
LIMIT_MIN_MAX_VALUE(
pidCalculation(&yawAttitudePidSettings, yawTransform(getYaw()),true,true,true),
-getGyroLimit(), getGyroLimit());
_DEBUG(DEBUG_ATTITUDE_PID_OUTPUT,
"(%s-%d) attitude pid output: roll=%.5f, pitch=%.5f, yaw=%.5f\n",
__func__, __LINE__, rollAttitudeOutput, pitchAttitudeOutput,
yawAttitudeOutput);
}
/**
* get output from altitude pid controler
*
* @param
* void
*
* @return
* void
*/
void getAltHoldAltPidOutput() {
altHoltAltOutput =
LIMIT_MIN_MAX_VALUE(
pidCalculation(&altHoldAltSettings, getCurrentAltHoldAltitude()-getTargetAlt(),true,true,true),
-getAltitudePidOutputLimitation(),
getAltitudePidOutputLimitation());
//_DEBUG(DEBUG_NORMAL,"getPidSp(&altHoldAltSettings)=%f\n",getPidSp(&altHoldAltSettings));
//_DEBUG(DEBUG_NORMAL,"getCurrentAltHoldAltitude=%f,getTargetAlt=%f\n",getCurrentAltHoldAltitude(),getTargetAlt());
//_DEBUG(DEBUG_NORMAL,"altHoltAltOutput=%f\n",altHoltAltOutput);
}
/**
* get throttle offset by acceleration
*
* @param
* void
*
* @return
* void
*/
float getThrottleOffsetByAcceleration(void) {
float output = 0.f;
setPidSp(&verticalAccelPidSettings, 0.f);
output = LIMIT_MIN_MAX_VALUE(pidCalculation(&verticalAccelPidSettings,
getVerticalAcceleration(),true,true,true), -maxThrottleOffset,
maxThrottleOffset);
//_DEBUG(DEBUG_NORMAL,"%s output =%f\n",__func__,output);
return output;
}
/**
* get throttle offset by altHold mechanism
*
* @param
* void
*
* @return
* void
*/
float getThrottleOffsetByAltHold(bool updateAltHoldOffset) {
float output = 0.f;
if (updateAltHoldOffset) {
getAltHoldAltPidOutput();
getAltHoldSpeedPidOutput(&output);
output = LIMIT_MIN_MAX_VALUE(output, -maxThrottleOffset,
maxThrottleOffset);
}
//_DEBUG(DEBUG_NORMAL,"output =%f\n",output);
return output;
}
/**
* this function controls motors by PID output
*
* @param
* void
*
* @return
* void
*
*/
void motorControler() {
float rollRateOutput = 0.f;
float rollCcw1 = 0.f;
float rollCcw2 = 0.f;
float rollCw1 = 0.f;
float rollCw2 = 0.f;
float pitchRateOutput = 0.f;
float pitchCcw1 = 0.f;
float pitchCcw2 = 0.f;
float pitchCw1 = 0.f;
float pitchCw2 = 0.f;
float yawRateOutput = 0.f;
float yawCcw1 = 0.f;
float yawCcw2 = 0.f;
float yawCw1 = 0.f;
float yawCw2 = 0.f;
float outCcw1 = 0.f;
float outCcw2 = 0.f;
float outCw1 = 0.f;
float outCw2 = 0.f;
float maxLimit = 0.f;
float minLimit = 0.f;
float accelThrottleOffset = 0.f;
float altholdThrottleOffset = 0.f;
float throttleOffset = 0.f;
float centerThrottle = 0.f;
altholdThrottleOffset = (getEnableAltHold() && getAltHoldIsReady()) ? getThrottleOffsetByAltHold(updateAltHold()) : 0.f;
accelThrottleOffset = getThrottleOffsetByAcceleration();
throttleOffset = altholdThrottleOffset + accelThrottleOffset;
centerThrottle = (float) getThrottlePowerLevel() + throttleOffset;
maxLimit = (float) min(centerThrottle + getAdjustPowerLeveRange(),
getMaxPowerLeve());
minLimit = (float) max(centerThrottle - getAdjustPowerLeveRange(),
getMinPowerLevel());
getAttitudePidOutput();
getRatePidOutput(&rollRateOutput, &pitchRateOutput, &yawRateOutput);
/*
* rollCa>0
* - CCW2 CW2 +
* X
* - CW1 CCW1 +
* F
*
* rollCa<0
* + CCW2 CW2 -
* X
* + CW1 CCW1 -
* F
*/
rollCcw1 = rollRateOutput;
rollCcw2 = -rollRateOutput;
rollCw1 = -rollRateOutput;
rollCw2 = rollRateOutput;
/*
* pitchCa>0
* + CCW2 CW2 +
* X
* - CW1 CCW1 -
* F
*
* pitchCa<0
* - CCW2 CW2 -
* X
* + CW1 CCW1 +
* F
*/
pitchCcw1 = -pitchRateOutput;
pitchCcw2 = pitchRateOutput;
pitchCw1 = -pitchRateOutput;
pitchCw2 = pitchRateOutput;
/*
* yawCa>0
* + CCW2 CW2 -
* X
* - CW1 CCW1 +
* F
*
* yawCa<0
* - CCW2 CW2 +
* X
* + CW1 CCW1 -
* F
*/
yawCcw1 = yawRateOutput;
yawCcw2 = yawRateOutput;
yawCw1 = -yawRateOutput;
yawCw2 = -yawRateOutput;
outCcw1 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCcw1 + pitchCcw1 + yawCcw1,
-getPidOutputLimitation(), getPidOutputLimitation());
outCcw2 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCcw2 + pitchCcw2 + yawCcw2,
-getPidOutputLimitation(), getPidOutputLimitation());
outCw1 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCw1 + pitchCw1 + yawCw1,
-getPidOutputLimitation(), getPidOutputLimitation());
outCw2 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCw2 + pitchCw2 + yawCw2,
-getPidOutputLimitation(), getPidOutputLimitation());
outCcw1 = getMotorGain(
SOFT_PWM_CCW1) * LIMIT_MIN_MAX_VALUE(outCcw1, minLimit, maxLimit);
outCcw2 = getMotorGain(
SOFT_PWM_CCW2) * LIMIT_MIN_MAX_VALUE(outCcw2, minLimit, maxLimit);
outCw1 = getMotorGain(
SOFT_PWM_CW1) * LIMIT_MIN_MAX_VALUE(outCw1, minLimit, maxLimit);
outCw2 = getMotorGain(
SOFT_PWM_CW2) * LIMIT_MIN_MAX_VALUE(outCw2, minLimit, maxLimit);
setupCcw1MotorPoewrLevel((unsigned short) outCcw1);
setupCcw2MotorPoewrLevel((unsigned short) outCcw2);
setupCw1MotorPoewrLevel((unsigned short) outCw1);
setupCw2MotorPoewrLevel((unsigned short) outCw2);
#if 0
_DEBUG(DEBUG_NORMAL,"outCcw1=%d,outCcw2=%d,outCw1=%d,outCw2=%d\n" ,
(unsigned short)outCcw1,
(unsigned short)outCcw2,
(unsigned short)outCw1,
(unsigned short)outCw2);
#endif
}
short processRadioMessages(char *buf, short lenth) {
short cmdIndex = -1;
char cmd[lenth];
char token2[lenth];
short i = 0;
short changeLeve = 0;
short parameter = 0;
float parameterF = 0.0;
float rollSpShift = 0;
float pitchSpShift = 0;
float yawShiftValue = 0;
float throttlePercentage = 0;
static unsigned long radio_last_tv3 = 0;
#if 0
unsigned long tv=millis();
Serial.println(tv-radio_last_tv3);
radio_last_tv3=tv;
// Serial.println(buf);
#endif
resetPacketAccCounter();
memset(cmd, '\0', sizeof(cmd));
for (i = 1; buf[i] != '#'; i++) {
cmd[i - 1] = buf[i];
}
qToken(cmd, token2,lenth, 0);
cmdIndex = atoi(token2);
switch (cmdIndex) {
case ON_OFF_FLY_SYSTEM_MESSAGE:
qToken(cmd, token2,lenth,ON_OFF_FLY);
parameter = atoi(token2);
if (1 == parameter) {
enableFlySystem();
} else {
disenableFlySystem();
}
break;
case PILOT_CONTROL_MESSAGE:
// printf("%s\n",buf);
qToken(cmd, token2,lenth, THROTTLE_LEVEL);
parameter = atoi(token2);
qToken(cmd, token2,lenth, ROLL_SP_SHIFT);
rollSpShift = atof(token2);
qToken(cmd, token2,lenth, PITCH_SP_SHIFT);
pitchSpShift = atof(token2);
qToken(cmd, token2,lenth, YAW_SP_SHIFT);
yawShiftValue = atof(token2);
//printf("factor=%d\n",(int)(((float)(parameter-100)/(float)100)*(float)(MAX_POWER_LEVEL-MIN_POWER_LEVEL)));
//printf("parameter=%d\n",parameter);
throttlePercentage = (float)parameter / 100.f;
parameter = getMinPowerLevel()
+ (int) (throttlePercentage
* (float) (getMaxPowerLeve()
- getMinPowerLevel())); /*(100~200)*10=1000~2000 us*/
//printf("getMaxPowerLeveRange()- getMinPowerLeveRange()=%f\n",getMaxPowerLeve()- getMinPowerLevel());
//printf("parameter=%d\n",parameter);
if (parameter > getMaxPowerLeve()
|| parameter < getMinPowerLevel()) {
printf( "break\n");
break;
}
if (true == flySystemIsEnable()) {
setThrottlePowerLevel(parameter);
if (getMinPowerLevel() == parameter) {
//printf("STOP\n");
resetPidRecord(&rollAttitudePidSettings);
resetPidRecord(&pitchAttitudePidSettings);
resetPidRecord(&yawAttitudePidSettings);
resetPidRecord(&rollRatePidSettings);
resetPidRecord(&pitchRatePidSettings);
resetPidRecord(&yawRatePidSettings);
setYawCenterPoint(0);
setPidSp(&yawAttitudePidSettings, 321.0);
} else {
if (getPidSp(&yawAttitudePidSettings) == 321.0) {
printf( "START Flying\n");
setYawCenterPoint(getYaw());
setPidSp(&yawAttitudePidSettings, 0);
}
setPidSp(&rollAttitudePidSettings,
LIMIT_MIN_MAX_VALUE(rollSpShift, -getAngularLimit(),
getAngularLimit()));
setPidSp(&pitchAttitudePidSettings,
LIMIT_MIN_MAX_VALUE(pitchSpShift, -getAngularLimit(),
getAngularLimit()));
setYawCenterPoint(getYawCenterPoint() + (yawShiftValue * 1.0));
}
}
//printf("throttle=%d roll=%f pitch=%f\n",parameter,rollSpShift,pitchSpShift);
break;
case SYSTEM_SETTING_MESSAGE:
printf( "%s " , buf);
qToken(cmd, token2,lenth, ANAGULAR_VELOVITY_LIMIT);
parameterF = atof(token2);
if (parameterF == 0) {
parameterF = 1;
}
setGyroLimit(parameterF);
printf( "Angular Velocity Limit: %4.3f\n",getGyroLimit());
/***/
qToken(cmd, token2,lenth, ROLL_CALIBRATION);
parameterF = atof(token2);
setPidSpShift(&rollAttitudePidSettings, parameterF);
printf( "Roll Angular Calibration: %4.3f\n",getPidSpShift(&rollAttitudePidSettings));
/***/
qToken(cmd, token2,lenth, PITCH_CALIBRATION);
parameterF = atof(token2);
setPidSpShift(&pitchAttitudePidSettings, parameterF);
printf( "Pitch Angular Calibration: %4.3f\n",getPidSpShift(&pitchAttitudePidSettings));
break;
case PID_SETTING_MESSAGE:
printf("%s\n", buf);
qToken(cmd, token2,lenth, ATTITUDE_ROLL_P);
parameterF = atof(token2);
setPGain(&rollAttitudePidSettings, parameterF);
printf( "Attitude Roll P Gain=%f\n", getPGain(&rollAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_ROLL_I);
parameterF = atof(token2);
setIGain(&rollAttitudePidSettings, parameterF);
printf("Attitude Roll I Gain=%f\n", getIGain(&rollAttitudePidSettings));
qToken(cmd, token2,lenth,ATTITUDE_ROLL_I_LIMIT );
parameterF = atof(token2);
setILimit(&rollAttitudePidSettings, parameterF);
printf( "Attitude Roll I Output Limit=%f\n",
getILimit(&rollAttitudePidSettings));
qToken(cmd, token2,lenth,ATTITUDE_ROLL_D );
parameterF = atof(token2);
setDGain(&rollAttitudePidSettings, parameterF);
printf("Attitude Roll D Gain=%f\n", getDGain(&rollAttitudePidSettings));
qToken(cmd, token2,lenth,ATTITUDE_PITCH_P );
parameterF = atof(token2);
setPGain(&pitchAttitudePidSettings, parameterF);
printf( "Attitude Pitch P Gain=%f\n", getPGain(&pitchAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_PITCH_I);
parameterF = atof(token2);
setIGain(&pitchAttitudePidSettings, parameterF);
printf( "Attitude Pitch I Gain=%f\n", getIGain(&pitchAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_PITCH_I_LIMIT);
parameterF = atof(token2);
setILimit(&pitchAttitudePidSettings, parameterF);
printf( "Attitude Pitch I Output Limit=%f\n",
getILimit(&pitchAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_PITCH_D);
parameterF = atof(token2);
setDGain(&pitchAttitudePidSettings, parameterF);
printf( "Attitude Pitch D Gain=%f\n", getDGain(&pitchAttitudePidSettings));
qToken(cmd, token2,lenth,ATTITUDE_YAW_P );
parameterF = atof(token2);
setPGain(&yawAttitudePidSettings, parameterF);
printf( "Attitude Yaw P Gain=%f\n", getPGain(&yawAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_YAW_I);
parameterF = atof(token2);
setIGain(&yawAttitudePidSettings, parameterF);
printf( "Attitude Yaw I Gain=%f\n", getIGain(&yawAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_YAW_I_LIMIT);
parameterF = atof(token2);
setILimit(&yawAttitudePidSettings, parameterF);
printf( "Attitude Yaw I Output Limit=%f\n", getILimit(&yawAttitudePidSettings));
qToken(cmd, token2,lenth, ATTITUDE_YAW_D);
parameterF = atof(token2);
setDGain(&yawAttitudePidSettings, parameterF);
printf( "Attitude Yaw D Gain=%f\n", getDGain(&yawAttitudePidSettings));
qToken(cmd, token2,lenth, RATE_ROLL_P);
parameterF = atof(token2);
setPGain(&rollRatePidSettings, parameterF);
printf( "Rate Roll P Gain=%f\n", getPGain(&rollRatePidSettings));
qToken(cmd, token2,lenth, RATE_ROLL_I);
parameterF = atof(token2);
setIGain(&rollRatePidSettings, parameterF);
printf( "Rate Roll I Gain=%f\n", getIGain(&rollRatePidSettings));
qToken(cmd, token2,lenth, RATE_ROLL_I_LIMIT);
parameterF = atof(token2);
setILimit(&rollRatePidSettings, parameterF);
printf( "Rate Roll I Output Limit=%f\n",
getILimit(&rollRatePidSettings));
qToken(cmd, token2,lenth,RATE_ROLL_D);
parameterF = atof(token2);
setDGain(&rollRatePidSettings, parameterF);
printf( "Rate Roll D Gain=%f\n", getDGain(&rollRatePidSettings));
qToken(cmd, token2,lenth,RATE_PITCH_P );
parameterF = atof(token2);
setPGain(&pitchRatePidSettings, parameterF);
printf( "Rate Pitch P Gain=%f\n", getPGain(&pitchRatePidSettings));
qToken(cmd, token2,lenth,RATE_PITCH_I );
parameterF = atof(token2);
setIGain(&pitchRatePidSettings, parameterF);
printf( "Rate Pitch I Gain=%f\n", getIGain(&pitchRatePidSettings));
qToken(cmd, token2,lenth,RATE_PITCH_I_LIMIT );
parameterF = atof(token2);
setILimit(&pitchRatePidSettings, parameterF);
printf( "Rate Pitch I Output Limit=%f\n",
getILimit(&pitchRatePidSettings));
qToken(cmd, token2,lenth, RATE_PITCH_D);
parameterF = atof(token2);
setDGain(&pitchRatePidSettings, parameterF);
printf( "Rate Pitch D Gain=%f\n", getDGain(&pitchRatePidSettings));
qToken(cmd, token2,lenth,RATE_YAW_P );
parameterF = atof(token2);
setPGain(&yawRatePidSettings, parameterF);
printf( "Rate Yaw P Gain=%f\n", getPGain(&yawRatePidSettings));
qToken(cmd, token2,lenth, RATE_YAW_I);
parameterF = atof(token2);
setIGain(&yawRatePidSettings, parameterF);
printf( "Rate Yaw I Gain=%f\n", getIGain(&yawRatePidSettings));
qToken(cmd, token2,lenth,RATE_YAW_I_LIMIT );
parameterF = atof(token2);
setILimit(&yawRatePidSettings, parameterF);
printf("Rate Yaw I Output Limit=%f\n",
getILimit(&yawRatePidSettings));
qToken(cmd, token2,lenth, RATE_YAW_D);
parameterF = atof(token2);
setDGain(&yawRatePidSettings, parameterF);
printf( "Rate Yaw D Gain=%f\n", getDGain(&yawRatePidSettings));
break;
default:
printf("unknow message\n");
}
}