int *my_div(int *a, int *b, int *i)
{
int **one;
int **two;
int *temp;
int len_a;
int *unv;
int *two_two;
len_a = my_strlen_int(a) + 10;
one = malloc(len_a * sizeof(int));
two = malloc(len_a * sizeof(int));
two_two = set_two_two();
unv = set_unv(len_a);
temp = b;
while (is_inf_or_equal(temp, a) != 0)
{
one[i[0]] = unv;
two[i[0]] = temp;
unv = my_mult(unv, two_two, i[1]);
temp = my_mult(temp, two_two, i[1]);
i[0] = i[0] + 1;
}
return (next_div(a, one, two, i));
}
void Signal_getEulerAnglesGyro( state_data_t *state, ImuData_t *measurments, control_time_t* time)
{
state->state_bf[pitch_bf] += my_mult( (int32_t) measurments->imu_data[gyro_y], time->value, TIME_SHIFT_FACTOR );
state->state_bf[roll_bf] += my_mult( (int32_t) measurments->imu_data[gyro_x], time->value, TIME_SHIFT_FACTOR );
state->state_bf[yaw_bf] += my_mult( (int32_t) measurments->imu_data[gyro_z], time->value, TIME_SHIFT_FACTOR );
// Indicate which fields are updated and valid.
state->state_valid_bf |= 1 << pitch_bf;
state->state_valid_bf |= 1 << roll_bf;
state->state_valid_bf |= 1 << yaw_bf;
}
void Signal_complemetaryFilter( state_data_t *state_accel, state_data_t *state_gyro, state_data_t *state )
{
/* Update gyro state to reduce accumulatory errors.*/
state->state_bf[pitch_bf] = my_mult( (int32_t) state_gyro->state_bf[pitch_bf], (int32_t) FILTER_COEFFICENT_GYRO_FP, FP_16_16_SHIFT ) +
my_mult( (int32_t) state_accel->state_bf[pitch_bf], (int32_t) FILTER_COEFFICENT_ACCEL_FP, FP_16_16_SHIFT );
state->state_bf[roll_bf] = my_mult( (int32_t) state_gyro->state_bf[roll_bf], (int32_t) FILTER_COEFFICENT_GYRO_FP, FP_16_16_SHIFT ) +
my_mult( (int32_t) state_accel->state_bf[roll_bf], (int32_t) FILTER_COEFFICENT_ACCEL_FP, FP_16_16_SHIFT );
state_gyro->state_bf[pitch_bf] = state->state_bf[pitch_bf];
state_gyro->state_bf[roll_bf] = state->state_bf[roll_bf];
// Indicate which fields are updated and valid.
state->state_valid_bf |= 1 << pitch_bf;
state->state_valid_bf |= 1 << roll_bf;
}
void Signal_getEulerAnglesAccel( state_data_t *state, ImuData_t *measurments )
{
int32_t numerator = 0;
int32_t denominator = 0;
// roll calculation
// tan(roll) = y / (Sign(z) * sqrt(z²+(m*x²)))
numerator = (int32_t) measurments->imu_data[accl_y];
denominator = SIGN(measurments->imu_data[accl_z])
* (int32_t) my_square_root(
my_square( measurments->imu_data[accl_z], FP_16_16_SHIFT) +
my_mult( my_square( measurments->imu_data[accl_x], FP_16_16_SHIFT), MY, FP_16_16_SHIFT),
FP_16_16_SHIFT );
state->state_bf[roll_bf] = atan2Lerp( numerator, denominator, FP_16_16_SHIFT);
// Since atan2Lerp has a domain of [0,2pi), and we want a domain of (-pi,pi) we need
// to remove 2pi if value > pi.
if(state->state_bf[roll_bf] > BRAD_PI)
{
state->state_bf[roll_bf] -= BRAD_2PI;
}
// pitch calculation
// tan(pitch) = -x / sqrt(y² + z²)
numerator = (int32_t) (-measurments->imu_data[accl_x]);
denominator = (int32_t) my_square_root( my_square( measurments->imu_data[accl_y], FP_16_16_SHIFT ) +
my_square( measurments->imu_data[accl_z], FP_16_16_SHIFT ) , FP_16_16_SHIFT);
state->state_bf[pitch_bf] = atan2Lerp( numerator, denominator, FP_16_16_SHIFT);
// Since atan2Lerp has a domain of [0,2pi), and we want a domain of (-pi,pi) we need
// to remove 2pi if value > pi.
if(state->state_bf[pitch_bf] > BRAD_PI)
{
state->state_bf[pitch_bf] -= BRAD_2PI;
}
// Indicate which fields are updated and valid.
state->state_valid_bf |= 1 << roll_bf;
state->state_valid_bf |= 1 << pitch_bf;
}
void Satellite_MapToSetpoint(Satellite_t* obj, spektrum_data_t *reciever_data, state_data_t *setpoint)
{
if( xSemaphoreTake( obj->xMutexParam, ( TickType_t )(1UL / portTICK_PERIOD_MS) ) == pdTRUE )
{
//subtract center point and convert to State scale.
setpoint->state_bf[thrust_sp] = (my_mult(my_mult((reciever_data->ch[0].value), SPECTRUM_TO_CTRL_THROTTLE, 0), obj->throMult, FP_16_16_SHIFT)); // THRO
setpoint->state_bf[yaw_rate_bf] = (my_mult(my_mult((reciever_data->ch[3].value - SATELLITE_CH_CENTER), SPECTRUM_TO_STATE_RATE, FP_16_16_SHIFT), obj->multiplier, FP_16_16_SHIFT)); // YAW
setpoint->state_bf[pitch_rate_bf] = (my_mult(my_mult((reciever_data->ch[2].value - SATELLITE_CH_CENTER), SPECTRUM_TO_STATE_RATE, FP_16_16_SHIFT), obj->multiplier, FP_16_16_SHIFT)); // PITCH
setpoint->state_bf[roll_rate_bf] = (my_mult(my_mult((reciever_data->ch[1].value - SATELLITE_CH_CENTER), SPECTRUM_TO_STATE_RATE, FP_16_16_SHIFT), obj->multiplier, FP_16_16_SHIFT)); // ROLL
setpoint->state_bf[pitch_bf] = (my_mult(my_mult((reciever_data->ch[2].value - SATELLITE_CH_CENTER), SPECTRUM_TO_STATE_ANGLE, FP_16_16_SHIFT), obj->multiplier, FP_16_16_SHIFT)); // PITCH
setpoint->state_bf[roll_bf] = (my_mult(my_mult((reciever_data->ch[1].value - SATELLITE_CH_CENTER), SPECTRUM_TO_STATE_ANGLE, FP_16_16_SHIFT), obj->multiplier, FP_16_16_SHIFT)); // ROLL
xSemaphoreGive(obj->xMutexParam);
}
return;
}
double MyGene::evaluate(MyGP &gp, string id, string className = "", int graphId){
double returnValue = 0.0;
if (isFunction()) {
switch (node->value())
{
case SUM:
returnValue = my_sum(NthMyChild(0)->evaluate(gp, id, className) , NthMyChild(1)->evaluate(gp, id, className));
break;
case SUB:
returnValue = my_sub(NthMyChild(0)->evaluate(gp, id, className), NthMyChild(1)->evaluate(gp, id, className));
break;
case MULT:
returnValue = my_mult(NthMyChild(0)->evaluate(gp, id, className), NthMyChild(1)->evaluate(gp, id, className));
break;
case DIV:
returnValue = my_div(NthMyChild(0)->evaluate(gp, id, className),NthMyChild(1)->evaluate(gp, id, className));
break;
case LOG: // log de tree0 na base tree1
returnValue = my_div(my_log(NthMyChild(0)->evaluate(gp, id, className)), my_log(NthMyChild(1)->evaluate(gp, id, className))) ;
break;
case POW: // pow de tree0 na base tree1
returnValue = my_pow(NthMyChild(0)->evaluate(gp, id, className), NthMyChild(1)->evaluate(gp, id, className)) ;
break;
default:
printf("Funcao: %c", node->value());
GPExitSystem ((char*) "MyGene::evaluate", (char*)"Undefined node value (function).");
}
}
if (isTerminal()) {
switch(node->value()) {
case DF_PER_TERM:
returnValue = 1.0 + my_log(stats->getDFperTerm(id));
break;
case DF_PER_CLASS:
returnValue = 1.0 + my_log(stats->getDFperClass(id, className));
break;
case TF_PER_TERM:
returnValue = 1.0 + my_log(stats->getTFperTerm(id));
break;
case TF_PER_CLASS:
returnValue = 1.0 + my_log(stats->getTFperClass(id, className)); //bao!
break;
case AM:
returnValue = stats->getAM(id, className);
break;
case MAX_AM:
returnValue = stats->getMaxAM(id);
break;
case DOMINANCE:
returnValue = stats->getDominance(id, className);
break;
case MAX_DOMINANCE:
returnValue = stats->getMaxDominance(id);
break;
case PROB_COND:
//esta igual ao AM!
returnValue = my_div(stats->getTFperClass(id, className) , stats->getTFperTerm(id));
//versao 2:
//returnValue = ( stats->getTFperClass(id, className) / ( stats->getSumTF() + 1.0)); //suavizada
//returnValue /= ( stats->getSumTFperClass(className) + 1.0) / (stats->getSumTF() + 1.0 );//suavizada
//versao 3: usada na dissertacao
// returnValue = stats->getTFperClass(id,className)/ stats->getSumTFperClass(className);
break;
case PROB_COND_NEG:
//returnValue = my_div(stats->getTFperClass(id, className) , stats->getTFperTerm(id));
returnValue = 1.0 - my_div(stats->getTFperClass(id, className) , stats->getTFperTerm(id));
break;
case GINI:
returnValue = stats->getGini(id);
break;
case IG:
returnValue = stats->getIG(id, className);
break;
case MAX_IG:
returnValue = stats->getMaxIG(id);
break;
case OR:
returnValue = stats->getOR(id, className);
break;
case MAX_OR:
returnValue = stats->getMaxOR(id);
break;
case IDF:
//returnValue = stats->getIDFclass(id, className);
returnValue = stats->getIDF(id);
break;
case TFIDF:
returnValue = stats->getTFIDF(id, className);
break;
case MAX_TFIDF:
returnValue = stats->getMaxTFIDF(id);
break;
case TFICF:
returnValue = stats->getTFICF(id, className);
break;
case MAX_TFICF:
returnValue = stats->getMaxTFICF(id);
break;
case CTD:
returnValue = stats->getCTD(id, className);
break;
case MAX_CTD:
returnValue = stats->getMaxCTD(id);
break;
case GSS:
returnValue = stats->getGSS(id, className);
break;
case MAX_GSS:
returnValue = stats->getMaxGSS(id);
break;
case CHI:
returnValue = stats->getCHI(id, className);
break;
case MAX_CHI:
returnValue = stats->getMaxCHI(id);
break;
case CC:
returnValue = stats->getCC(id, className);
break;
case MAX_CC:
returnValue = stats->getMaxCC(id);
break;
case CE:
returnValue = stats->getCE(id);
break;
case NEIGHBORHOOD1:
returnValue = stats->getNeighborhoodSize1(id,className,graphId);
break;
case NEIGHBORHOOD2:
returnValue = stats->getNeighborhoodSize2(id,className,graphId);
break;
case NEIGHBORHOOD3:
returnValue = stats->getNeighborhoodSize3(id,className,graphId);
break;
case HUBSCORE:
returnValue = stats->getHubScore(id, className, graphId);
break;
case AUTHORITY:
returnValue = stats->getAuthority(id, className, graphId);
break;
case EIGENVECTOR:
returnValue = stats->getEigenVectorCentrality(id, className, graphId);
break;
case CLOSENESS:
returnValue = stats->getCloseness(id, className, graphId);
break;
case STRENGTH:
returnValue = stats->getStrength(id, className, graphId);
break;
case CONSTRAINT:
returnValue = stats->getConstraint(id, className, graphId);
break;
case PAGERANK:
returnValue = stats->getPageRank(id, className, graphId);
break;
case BETWEENNESS:
returnValue = stats->getBetweenness(id, className, graphId);
break;
case BIBCOUPLING:
returnValue = stats->getBibCoupling(id, className, graphId);
break;
case COCITATION:
returnValue = stats->getCoCitation(id, className, graphId);
break;
case JACCARDSIMILARITY:
returnValue = stats->getJaccardSimilarity(id, className, graphId);
break;
case DICESIMILARITY:
returnValue = stats->getDiceSimilarity(id, className, graphId);
break;
case INVERSELOGSIMILARITY:
returnValue = stats->getInverseLogSimilarity(id, className, graphId);
break;
case AVGNEIGHBORHOODDEGREE:
returnValue = stats->getAvgNearstNeighborDegree(id, className, graphId);
break;
case NUM1:
returnValue = 1.0;
break;
case NUM2:
returnValue = 2.0;
break;
case NUM3:
returnValue = 3.0;
break;
default:
cout << " Terminal: " << node->value() << endl;
GPExitSystem ((char*)"MyGene::evaluate",(char*) "Undefined node value (terminal).");
}
}
if(isnan(returnValue)){
cerr<<"Not a number found! Stop it!"<<endl;
cerr<<"Valor do nodo = " << node->value()<<endl;
returnValue=0;
//exit(-1);
}
//DEBUG(cout<<" valor = "<<returnValue <<endl);;
return returnValue;
}
