void doFlightControl(int16_t* IMUData,double* angle,int16_t* PPMIn,int16_t* PPMOut){
updateYaw(PPMIn[3]*_constants[0][3]/15);
if (_isEngeinsOnFlag){
// lets define four control signals:
// u0 to control the colective
// u1 to control the roll
// u2 to control the pitch
// u3 to conrol the yaw.
int16_t u[4];
u[0] = (PPMIn[2]+145)*_constants[0][0]; //colective
u[1] = PPMIn[1]*_constants[0][1]+(_constants[1][1]*angle[0]+_constants[2][1]*angle[3]); //roll
u[2] = PPMIn[0]*_constants[0][2]+(_constants[1][2]*angle[1]+_constants[2][2]*angle[4]); //pitch
u[3] = (_constants[1][3]*angle[2]+_constants[2][3]*angle[5]); //yaw
setOutputsData(u,20,23);
////////////////////////////////////////////
for (int i=0;i<4;i++){
PPMOut[i] = 0;
for (int j=0;j<4;j++){
PPMOut[i] += mixer[j][i]*u[j];
}
PPMOut[i] = PPMOut[i]/10;
SPEED_LIMITER(PPMOut[i]);
}
} else {
for (int i=0;i<4;i++)
PPMOut[i] = 0;
}
setOutputsData(PPMIn,12,15);
setOutputData(_control,32);
setOutputData(_isEngeinsOnFlag,33);
}
void Estimator(int16_t *IMUDataInt,double *angle)
{
int16_t * imuBiases;
double IMUData[6];
imuBiases = GetIMUBiases();
// subtract the bias from acc and gyro:
for (int j=0; j<6; j++)
IMUData[j]=IMUDataInt[j] - imuBiases[j]/20.0;
IMUData[2] +=256; //need to feal the gravity
double accPhi = atan2(IMUData[1],IMUData[2]);
double accTeta = atan2(IMUData[0],sqrt(IMUData[1]*IMUData[1]+IMUData[2]*IMUData[2]));
double magPsi = atan2(IMUDataInt[7],-IMUDataInt[6]);
double p= IMUData[3]*GYRO_FACTOR;
double q= IMUData[4]*GYRO_FACTOR;
double r= IMUData[5]*GYRO_FACTOR;
double sinphi = sin(phi );
double cosphi = cos(phi );
double sinteta = sin(teta);
double costeta = cos(teta);
double dphi = (p+((q*sinphi)+(r*cosphi))*(sinteta/costeta) );
double dteta = -((q*cosphi)-(r*sinphi));
double dpsi = -(q*sinphi+r*cosphi)/costeta;
double AccNorm = IMUData[0]*IMUData[0] + IMUData[1]*IMUData[1]+IMUData[2]*IMUData[2];
double AccFactor = sqrt( (62500.0 - AccNorm)*(62500.0 - AccNorm));
AccFactor = (AccFactor > 1500) ? 0 : (1500 - AccFactor)/1500;
phi = (phi + dphi*SYNC_PERIOD)* (1 - AccFactor*0.1) + AccFactor*0.1*accPhi; // int type
teta = (teta +dteta*SYNC_PERIOD)*(1 - AccFactor*0.1) + AccFactor*0.1*accTeta; // int type
psi = (psi + dpsi*SYNC_PERIOD)*0.9999 + 0.0001*magPsi; //psi ;
ANGLELIMITER(phi);
ANGLELIMITER(teta);
ANGLELIMITER(psi );
angle[0] = phi;
angle[1] = teta;
angle[2] = psi;
angle[3] = dphi;
angle[4] = dteta;
angle[5] = dpsi;
for(uint8_t i=0;i<3;i++){
setOutputData((angle[i]*180)/3.14,i+9);
}
}
int itkFiltersComponentSizeThresholdProcess::updateProcess(medAbstractData* inputData)
{
dtkSmartPointer<medAbstractData> adjustedInputData = inputData;
if (std::is_floating_point<typename InputImageType::PixelType>::value)
{
adjustedInputData = castToOutputType<InputImageType>(inputData);;
}
typename InputImageType::Pointer inputImage = static_cast<InputImageType*>(inputData->data());
typedef itk::ConnectedComponentImageFilter <InputImageType, OutputImageType> ConnectedComponentFilterType;
typename ConnectedComponentFilterType::Pointer connectedComponentFilter = ConnectedComponentFilterType::New();
connectedComponentFilter->SetInput(inputImage);
connectedComponentFilter->Update();
// RELABEL COMPONENTS according to their sizes (0:largest(background))
typedef itk::RelabelComponentImageFilter<OutputImageType, OutputImageType> FilterType;
typename FilterType::Pointer relabelFilter = FilterType::New();
relabelFilter->SetInput(connectedComponentFilter->GetOutput());
relabelFilter->SetMinimumObjectSize(d->minimumSize);
relabelFilter->Update();
// BINARY FILTER
typedef itk::BinaryThresholdImageFilter <OutputImageType, OutputImageType> BinaryThresholdImageFilterType;
typename BinaryThresholdImageFilterType::Pointer thresholdFilter
= BinaryThresholdImageFilterType::New();
thresholdFilter->SetInput(relabelFilter->GetOutput());
thresholdFilter->SetUpperThreshold(0);
thresholdFilter->SetInsideValue(0);
thresholdFilter->SetOutsideValue(1);
thresholdFilter->Update();
itk::CStyleCommand::Pointer callback = itk::CStyleCommand::New();
callback->SetClientData ( ( void * ) this );
callback->SetCallback ( itkFiltersProcessBase::eventCallback );
connectedComponentFilter->AddObserver ( itk::ProgressEvent(), callback );
setOutputData(medAbstractDataFactory::instance()->createSmartPointer(medUtilitiesITK::itkDataImageId<OutputImageType>()));
getOutputData()->setData ( thresholdFilter->GetOutput() );
QString newSeriesDescription = "connectedComponent " + QString::number(d->minimumSize);
medUtilities::setDerivedMetaData(getOutputData(), inputData, newSeriesDescription);
return DTK_SUCCEED;
}
void Estimator2(int16_t *IMUDataInt,double *angle)
{
int16_t * imuBiases;
int16_t IMUData[6];
imuBiases = GetIMUBiases();
// subtract the bias from acc and gyro:
for (int j=0; j<6; j++)
IMUData[j]=IMUDataInt[j] - imuBiases[j]/20.0;
IMUData[2] +=256; //need to feal the gravity
int16_t accPhi = fixATan2(IMUData[1],IMUData[2]);
int16_t accTeta = fixATan2(IMUData[0],intSqrt((int32_t)IMUData[1]*IMUData[1]+(int32_t)IMUData[2]*IMUData[2]));
angle[0] = accPhi;
angle[1] = accTeta;
for(uint8_t i=0;i<2;i++){
setOutputData(angle[i]*180 / PI,i+9);
}
}
