Picopter 的研究 (1)

inline void ControlClass::updatePWM_(float* throttle, float* pitch, float* roll, float* yaw) {

   PICInterface.pwmwidths.frontleft = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) - *pitch + *roll - *yaw + altitudePID.output;
    PICInterface.pwmwidths.frontright = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) - *pitch - *roll + *yaw + altitudePID.output;
    PICInterface.pwmwidths.rearright = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) + *pitch - *roll - *yaw + altitudePID.output;
    PICInterface.pwmwidths.rearleft = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) + *pitch + *roll + *yaw + altitudePID.output;

}

看的出來定高模式....是在PWM輸出的地方加入一個   altitudePID.output.....

用 SW2 來決定pid mode.....  

SW2 -> false -> rateControl_

SW2 -> true ->  attitudeControl_

void ControlClass::update() {

    if(motorTesting_ == false) {

        if(PICInterface.rx.sw2 == false) {//in rate mode

            rateControl_(&PICInterface.rx.pitchRateDem, &PICInterface.rx.rollRateDem, &PICInterface.rx.yawRateDem);

        } else if(PICInterface.rx.sw2 == true) { //in attitude mode

            attitudeControl_(&PICInterface.rx.pitchDem, &PICInterface.rx.rollDem, &PICInterface.rx.yawRateDem);

        }

    } else {

        incrementMotorTest_();

    }

    //        evaluateAltitudeControl_(); //Checks to see if altitude control if required, and performs if necessary

}

  

void ControlClass::rateControl_(float* pitchrate, float* rollrate, float* yawrate) {

    ratePitchPID.calculate(&AHRS.calibratedData.p, pitchrate, &Timer.dt);

    rateRollPID.calculate(&AHRS.calibratedData.q, rollrate, &Timer.dt);

    rateYawPID.calculate(&AHRS.calibratedData.r, yawrate, &Timer.dt);

    updatePWM_(&PICInterface.rx.throttleDem, &ratePitchPID.output, &rateRollPID.output, &rateYawPID.output);

}

void ControlClass::attitudeControl_(float* targetPitch, float* targetRoll, float* targetYawRate) {

    attitudePitchPID.calculate(&AHRS.orientation.pitch, targetPitch, &Timer.dt);

    attitudeRollPID.calculate(&AHRS.orientation.roll, targetRoll, &Timer.dt);

    rateControl_(&attitudePitchPID.output, &attitudeRollPID.output, targetYawRate);

}

rate control mode.... 

input 是  AHRS.calibratedData.p , AHRS.calibratedData.q,  AHRS.calibratedData.r

            pitchrate,  rollrate, yawrate

attitudeControl mode....

input 是   AHRS.orientation.pitch, AHRS.orientation.roll,  

                                     

然後再呼叫   rate control mode

再看一下    AHRS algorithm  ,  main function   update() 

void AHRSClass::update() {

    getSensors_();

    calibrateData_();

    temperatureCompensate_();

    fuse_();

}

void AHRSClass::getSensors_() {

    MPU6050.getSensors(&rawData_);

    HMC5883L.getField(&rawData_);

    MS5611.getPressure(&rawData_.pressure);

}

呼叫3各class 去  get data....

MPU6050.getSensors(&rawData_);   ->  

    rawData_.x ;

    rawData_.y ;

    rawData_.z ;

   

    rawData_.p ; (pitch)

    rawData_.q ; (roll)

    rawData_.r ;  (yaw)

HMC5883L.getField(&rawData_);  ->

    rawData_.mag_x ;

    rawData_.mag_y ;

    rawData_.mag_z ;

MS5611.getPressure(&rawData_.pressure) ->

   

   rawData_.pressure