float Height_Ctrl(float T,float thr,u8 ready,float en) { static u8 step,speed_cnt,height_cnt; if(ready == 0) { ex_i_en = ex_i_en_f = 0; en = 0; thr_take_off = 0; thr_take_off_f = 0; } switch(step) { case 0: { //step = 1; break; } case 1: { step = 2; break; } case 2: { step = 3; break; } case 3: { step = 4; break; } case 4: { step = 0; break; } default:break; } /*飞行中初次进入定高模式切换处理*/ if(ABS(en - en_old) > 0.5f)//从非定高切换到定高 { if(thr_take_off<10)//未计算起飞油门 { if(thr_set > -150) { thr_take_off = 400; } } en_old = en; } /*定高控制*/ //h_pid_init(); thr_set = my_deathzoom_2(my_deathzoom((thr - 500),0,40),0,10); if(thr_set>0) { set_speed_t = thr_set/450 * MAX_VERTICAL_SPEED_UP; if(thr_set>100) { ex_i_en_f = 1; if(!thr_take_off_f) { thr_take_off_f = 1; //用户可能想要起飞 thr_take_off = 350; //直接赋值 一次 } } } else { if(ex_i_en_f == 1) { ex_i_en = 1; } set_speed_t = thr_set/450 * MAX_VERTICAL_SPEED_DW; } set_speed_t = LIMIT(set_speed_t,-MAX_VERTICAL_SPEED_DW,MAX_VERTICAL_SPEED_UP); //exp_speed =my_pow_2_curve(exp_speed_t,0.45f,MAX_VERTICAL_SPEED); LPF_1_(10.0f,T,my_pow_2_curve(set_speed_t,0.25f,MAX_VERTICAL_SPEED_DW),set_speed); set_speed = LIMIT(set_speed,-MAX_VERTICAL_SPEED_DW,MAX_VERTICAL_SPEED_UP); ///////////////////////////////////////////////////////////////////////////////// baro_ctrl(T,&hc_value); //高度数据获取: 气压计数据 ///////////////////////////////////////////////////////////////////////////////// //计算高度误差(可加滤波) set_height_em += (set_speed - hc_value.m_speed) *T; set_height_em = LIMIT(set_height_em,-5000 *ex_i_en,5000 *ex_i_en); set_height_e += (set_speed - 1.05f *hc_value.fusion_speed) *T; set_height_e = LIMIT(set_height_e,-5000 *ex_i_en,5000 *ex_i_en); LPF_1_(0.05f,T,set_height_em,set_height_e); ///////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////// if(en < 0.1f) { exp_speed = hc_value.fusion_speed; exp_acc = hc_value.fusion_acc; } ///////////////////////////////////////////////////////////////////////////////// float acc_i_lim; acc_i_lim = safe_div(150,h_acc_arg.ki,0); fb_speed_old = fb_speed; fb_speed = hc_value.fusion_speed; fb_acc = safe_div(fb_speed - fb_speed_old,T,0); thr_pid_out = PID_calculate( T, //周期 exp_acc, //前馈 exp_acc, //期望值(设定值) fb_acc, //反馈值 &h_acc_arg, //PID参数结构体 &h_acc_val, //PID数据结构体 acc_i_lim*en //integration limit,积分限幅 ); //输出 //step_filter(1000 *T,thr_pid_out,thr_pid_out_dlim); //起飞油门 if(h_acc_val.err_i > (acc_i_lim * 0.2f)) { if(thr_take_off<THR_TAKE_OFF_LIMIT) { thr_take_off += 150 *T; h_acc_val.err_i -= safe_div(150,h_acc_arg.ki,0) *T; } } else if(h_acc_val.err_i < (-acc_i_lim * 0.2f)) { if(thr_take_off>0) { thr_take_off -= 150 *T; h_acc_val.err_i += safe_div(150,h_acc_arg.ki,0) *T; } } thr_take_off = LIMIT(thr_take_off,0,THR_TAKE_OFF_LIMIT); //一半 //油门补偿 tilted_fix = safe_div(1,LIMIT(reference_v.z,0.707f,1),0); //45度内补偿 //油门输出 thr_out = (thr_pid_out + tilted_fix *(thr_take_off) ); thr_out = LIMIT(thr_out,0,1000); ///////////////////////////////////////////////////////////////////////////////// static float dT,dT2; dT += T; speed_cnt++; if(speed_cnt>=10) //u8 20ms { exp_acc = PID_calculate( dT, //周期 exp_speed, //前馈 (set_speed + exp_speed), //期望值(设定值) hc_value.fusion_speed, //反馈值 &h_speed_arg, //PID参数结构体 &h_speed_val, //PID数据结构体 500 *en //integration limit,积分限幅 ); //输出 exp_acc = LIMIT(exp_acc,-3000,3000); //integra_fix += (exp_speed - hc_value.m_speed) *dT; //integra_fix = LIMIT(integra_fix,-1500 *en,1500 *en); //LPF_1_(0.5f,dT,integra_fix,h_speed_val.err_i); dT2 += dT; height_cnt++; if(height_cnt>=10) //200ms { ///////////////////////////////////// exp_speed = PID_calculate( dT2, //周期 0, //前馈 0, //期望值(设定值) -set_height_e, //反馈值 &h_height_arg, //PID参数结构体 &h_height_val, //PID数据结构体 1500 *en //integration limit,积分限幅 ); //输出 exp_speed = LIMIT(exp_speed,-300,300); ///////////////////////////////////// dT2 = 0; height_cnt = 0; } speed_cnt = 0; dT = 0; } ///////////////////////////////////////////////////////////////////////////////// if(step==0) { step = 1; } if(en < 0.1f) { return (thr); } else { return (thr_out); } }
static unsigned default_promote_level(struct smq_policy *mq) { /* * The promote level depends on the current performance of the * cache. * * If the cache is performing badly, then we can't afford * to promote much without causing performance to drop below that * of the origin device. * * If the cache is performing well, then we don't need to promote * much. If it isn't broken, don't fix it. * * If the cache is middling then we promote more. * * This scheme reminds me of a graph of entropy vs probability of a * binary variable. */ static unsigned table[] = {1, 1, 1, 2, 4, 6, 7, 8, 7, 6, 4, 4, 3, 3, 2, 2, 1}; unsigned hits = mq->cache_stats.hits; unsigned misses = mq->cache_stats.misses; unsigned index = safe_div(hits << 4u, hits + misses); return table[index]; }
ssize_t mphf_chm_imp_rebuild (mphf_t *mphf){ // {{{ ssize_t ret; uint64_t nelements; uint64_t capacity_curr; chm_imp_t *data = (chm_imp_t *)&mphf->data; // | empty array | existing array // -----------------+---------------------+-------------------------- // params.nelements | 0 | calculated array size // params.capacity | 0 | array defined capacity // -----------------+---------------------+-------------------------- capacity_curr = data->params.capacity; if( safe_div(&capacity_curr, capacity_curr, 100) < 0 || safe_mul(&capacity_curr, capacity_curr, REBUILD_CONST) < 0 ) capacity_curr = data->params.capacity; nelements = data->params.nelements; if(nelements >= capacity_curr){ // expand array nelements = MAX(MAX(data->params.capacity, data->nelements_min), nelements); nelements += data->nelements_step; nelements *= data->nelements_mul; #ifdef MPHF_DEBUG printf("mphf expand rebuild on %x (%d) elements\n", (int)nelements, (int)nelements); #endif }else{ // ordinary rebuild nelements = data->params.capacity; #ifdef MPHF_DEBUG printf("mphf normal rebuild on %x (%d) elements\n", (int)nelements, (int)nelements); #endif } // set .params data->params.hash1 = random(); data->params.hash2 = random(); data->params.nelements = 0; if( (ret = chm_imp_configure_r(mphf, nelements)) < 0) return ret; // reinit files if( (ret = chm_imp_file_init(mphf)) < 0) return ret; // save new .params if( (ret = chm_imp_param_write(mphf)) < 0) return ret; return 0; } // }}}
static ssize_t chm_imp_configure_r(mphf_t *mphf, uintmax_t capacity){ // {{{ uintmax_t nvertex; chm_imp_t *data = (chm_imp_t *)&mphf->data; if(capacity == 0) capacity = 1; // at least one element // nvertex = (2.09 * capacity) if(safe_mul(&nvertex, capacity, CHM_CONST) < 0){ safe_div(&nvertex, capacity, 100); if(safe_mul(&nvertex, nvertex, CHM_CONST) < 0) return error("too many elements"); } safe_div(&nvertex, nvertex, 100); data->nvertex = nvertex; data->params.capacity = capacity; data->bt_vertex = BITS_TO_BYTES( log_any(nvertex, 2) ); return 0; } // }}}
/* * There are times when we don't have any confidence in the hotspot queue. * Such as when a fresh cache is created and the blocks have been spread * out across the levels, or if an io load changes. We detect this by * seeing how often a lookup is in the top levels of the hotspot queue. */ static enum performance stats_assess(struct stats *s) { unsigned confidence = safe_div(s->hits << FP_SHIFT, s->hits + s->misses); if (confidence < SIXTEENTH) return Q_POOR; else if (confidence < EIGHTH) return Q_FAIR; else return Q_WELL; }
/* added according to: http://www.simplefilter.de/en/basics/mixmods.html */ void gimp_operation_layer_mode_blend_vivid_light (const gfloat *in, const gfloat *layer, gfloat *comp, gint samples) { while (samples--) { if (in[ALPHA] != 0.0f && layer[ALPHA] != 0.0f) { gint c; for (c = 0; c < 3; c++) { gfloat val; if (layer[c] <= 0.5f) { val = 1.0f - safe_div (1.0f - in[c], 2.0f * layer[c]); val = MAX (val, 0.0f); } else { val = safe_div (in[c], 2.0f * (1.0f - layer[c])); val = MIN (val, 1.0f); } comp[c] = val; } } comp[ALPHA] = layer[ALPHA]; comp += 4; layer += 4; in += 4; } }
static void q_set_targets_subrange_(struct queue *q, unsigned nr_elts, unsigned lbegin, unsigned lend) { unsigned level, nr_levels, entries_per_level, remainder; BUG_ON(lbegin > lend); BUG_ON(lend > q->nr_levels); nr_levels = lend - lbegin; entries_per_level = safe_div(nr_elts, nr_levels); remainder = safe_mod(nr_elts, nr_levels); for (level = lbegin; level < lend; level++) q->target_count[level] = (level < (lbegin + remainder)) ? entries_per_level + 1u : entries_per_level; }
void gimp_operation_layer_mode_blend_luminance (const gfloat *in, const gfloat *layer, gfloat *comp, gint samples) { static const Babl *fish; gfloat *scratch; gfloat *in_Y; gfloat *layer_Y; if (! fish) fish = babl_fish ("RGBA float", "Y float"); scratch = gegl_scratch_new (gfloat, 2 * samples); in_Y = scratch; layer_Y = scratch + samples; babl_process (fish, in, in_Y, samples); babl_process (fish, layer, layer_Y, samples); while (samples--) { if (layer[ALPHA] != 0.0f && in[ALPHA] != 0.0f) { gfloat ratio = safe_div (layer_Y[0], in_Y[0]); gint c; for (c = 0; c < 3; c ++) comp[c] = in[c] * ratio; } comp[ALPHA] = layer[ALPHA]; comp += 4; in += 4; layer += 4; in_Y ++; layer_Y ++; } gegl_scratch_free (scratch); }
void gimp_operation_layer_mode_blend_dodge (const gfloat *in, const gfloat *layer, gfloat *comp, gint samples) { while (samples--) { if (in[ALPHA] != 0.0f && layer[ALPHA] != 0.0f) { gint c; for (c = 0; c < 3; c++) comp[c] = safe_div (in[c], 1.0f - layer[c]); } comp[ALPHA] = layer[ALPHA]; comp += 4; layer += 4; in += 4; } }
static double percent (double x, double y) { return safe_div (100.0 * x, y); }