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); }
