/**
 * 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
}
Beispiel #7
0
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");
  }

}