void PID_path_arc(int32_t x,int32_t y,int32_t sx, int32_t sy , int32_t sa)
{
PID_target_X = x;
PID_target_Y = y;
PID_start_X = sx;
PID_start_Y = sy;
PID_start_angle = sa;
int32_t a = (((x-sx)*int_cos(sa)) - (y-sy)*int_sin(sa))/10000;
int32_t b = (((x-sx)*int_sin(sa)) + (y-sy)*int_cos(sa))/10000;
if(b < 0) b = -b;
if(a >= 0)
PID_dir = 1; // right
else
{
PID_dir = -1;
a = -a;
}
PID_radius = (b * b / a + a)>>1;
//int16_t angle = int_arccos((r - a) / r);
PID_center_X = sx + PID_dir * PID_radius * int_cos(sa)/10000;
PID_center_Y = sy - PID_dir * PID_radius * int_sin(sa)/10000;
PID_status = 7;
}
int PID_arc_done(void)
{
if(((position_get_X() - PID_target_X)*int_sin(PID_desire_angle) + (position_get_Y() - PID_target_Y)*int_cos(PID_desire_angle)>0)
&& ((position_get_X() - PID_target_X)*(position_get_X() - PID_target_X)+(position_get_Y() - PID_target_Y)*(position_get_Y() - PID_target_Y)<=50000))
return 1;
else
return 0;
}
static int init_demo(const char *filename)
{
int i, j;
int h;
int radian;
char line[3 * W];
FILE *input_file;
input_file = fopen(filename, "rb");
if (!input_file) {
perror(filename);
return 1;
}
if (fread(line, 1, 15, input_file) != 15)
return 1;
for (i = 0; i < H; i++) {
if (fread(line, 1, 3 * W, input_file) != 3 * W)
return 1;
for (j = 0; j < W; j++) {
tab_r[W * i + j] = line[3 * j ];
tab_g[W * i + j] = line[3 * j + 1];
tab_b[W * i + j] = line[3 * j + 2];
}
}
fclose(input_file);
/* tables sin/cos */
for (i = 0; i < 360; i++) {
radian = 2 * i * MY_PI / 360;
h = 2 * FIXP + int_sin(radian);
h_cos[i] = h * int_sin(radian + MY_PI / 2) / 2 / FIXP;
h_sin[i] = h * int_sin(radian) / 2 / FIXP;
}
return 0;
}
void PID_set_target_distance(int32_t pos)
{
PID_disable();
PID_prev_error =0;
PID_error =0;
PID_integration = 0;
PID_u = 0;
PID_desire_angle = position_get_angle();//modified
PID_start_X = position_get_X();
PID_start_Y = position_get_Y();
PID_cos = int_cos(PID_desire_angle);
PID_sin = int_sin(PID_desire_angle);
PID_target_X = PID_start_X + pos*int_sin(PID_desire_angle)/10000;
PID_target_Y = PID_start_Y + pos*int_cos(PID_desire_angle)/10000;
if(pos < 0)
PID_desire_angle = (PID_desire_angle+180)%360;
PID_set_status(3);
}
void PID_set_target_angle(int16_t angle)
{
PID_disable();
PID_prev_error =0;
PID_error =0;
PID_integration = 0;
PID_u = 0;
PID_desire_angle = angle;
PID_start_X = position_get_X();
PID_start_Y = position_get_Y();
PID_cos = int_cos(PID_desire_angle);
PID_sin = int_sin(PID_desire_angle);
PID_set_status(1);
}
void PID_set_target_XYAngle_back(int32_t X,int32_t Y,int32_t angle)
{
PID_disable();
PID_prev_error =0;
PID_error =0;
PID_integration = 0;
PID_u = 0;
PID_start_X = position_get_X();
PID_start_Y = position_get_Y();
PID_target_X = X;
PID_target_Y = Y;
PID_desire_angle = angle;
PID_cos = int_cos((angle + 180)%360);
PID_sin = int_sin((angle + 180)%360);
PID_set_status(3);
}
void PID_calculation_u(void)
{
static int32_t PID_current_angle; //chu zao yin
static int32_t PID_current_X;
static int32_t PID_current_Y;
static int16_t PID_angle;
static int32_t d = 0;
static int16_t diff_x=0, diff_y=0;
static int16_t sita=0, arfa=0;
static int8_t qua = 0;
if(PID_status)
{
PID_current_angle = position_get_angle();//modified
PID_current_angle = (PID_current_angle>180)?(PID_current_angle-360):PID_current_angle;
PID_current_X = position_get_X();
PID_current_Y = position_get_Y();
if(PID_status&_BV(0))
{
if (PID_status&_BV(2)) {//arc mode
diff_x = PID_current_X - PID_center_X;
diff_y = PID_current_Y - PID_center_Y;
if (PID_dir==1) {
if(diff_x<0 && diff_y >= 0){ //0-90
sita = int_arc_tan_ver2(diff_y,-diff_x);
}
else if(diff_x>=0 && diff_y>=0){//90-180
sita = int_arc_tan_ver2(diff_x,diff_y) + 90;
}
else if(diff_x>=0 && diff_y<0){//-180 - -90
sita = int_arc_tan_ver2(diff_x,diff_y) - 90;
}
else if(diff_x<0 && diff_y<0){ //-90 - 0
sita = int_arc_tan_ver2(-diff_y,diff_x);
}
}
else if (PID_dir==-1) {
if(diff_x>0 && diff_y>=0){ //0-90
sita = int_arc_tan_ver2(diff_y,diff_x);
}
else if(diff_x<=0 && diff_y>=0){//90-180
sita = int_arc_tan_ver2(-diff_x,diff_y) + 90;
}
else if(diff_x<=0 && diff_y<0){//-180 - -90
sita = int_arc_tan_ver2(-diff_x,diff_y) - 90;
}
else if(diff_x>0 && diff_y<0){ //-90 - 0
sita = int_arc_tan_ver2(diff_y,diff_x);
}
}
qua = sita/45;
switch(qua){
case 0:
case 3:
case -3:
case 4:
case -4:
d = PID_radius + (int32_t)diff_x*10000/int_cos(sita)*(int32_t)PID_dir;
break;
case 1:
case 2:
case -1:
case -2:
d = PID_radius - (int32_t)diff_y*10000/int_sin(sita);
break;
}
PID_desire_angle=PID_start_angle+sita*(int32_t)PID_dir;
arfa = int_arc_sin(10000*d/RAD);
PID_error = arfa*(int32_t)PID_dir + PID_current_angle - sita*(int32_t)PID_dir;
}
else {//line mode
PID_error_position = ((PID_current_X - PID_start_X)*PID_cos - (PID_current_Y - PID_start_Y)*PID_sin)/10000;
PID_angle = int_arc_sin(10000*PID_error_position/RAD);
//To fix the angle difference problem near negative y-axis(jumping from 17x to -17x)
int16_t tmp=PID_current_angle - PID_desire_angle;
if (tmp>=180)
tmp -= 360;
else if (tmp<=-180)
tmp += 360;
PID_error= tmp + PID_angle;
}
PID_integration = KI * PID_error + PID_integration;
//Integration error limit
if(PID_integration >= IL)
PID_integration = IL;
PID_u =KP * PID_prev_error + PID_integration +KD * (PID_error - PID_prev_error);
//Error limit
if(PID_u > UL)
PID_u = UL;
else if(PID_u < - UL)
PID_u = -UL;
PID_prev_error = PID_error;
}
lcd_clear();
lcd_printxys(0,0,"X:%d",PID_current_X);
lcd_printxys(10,0,"Y:%d",PID_current_Y);
lcd_printxys(0,1,"A:%d",arfa);
lcd_printxys(0,2,"u: %ld",PID_u);
lcd_printxys(10,2,"pe: %d",PID_error);
}
}
int rotate_filter_frame(RotContext *rot, AVFrame *in,int frame_count_out, AVFrame* &out)
{
int angle_int, s, c, plane;
double res;
rot->var_values[ROTATE_VAR_N] = frame_count_out;
rot->var_values[ROTATE_VAR_T] = 0;//TS2T(in->pts, inlink->time_base);
rot->angle = res = av_expr_eval(rot->angle_expr, rot->var_values, rot);
av_log(NULL, AV_LOG_DEBUG, "n:%f time:%f angle:%f/PI\n",
rot->var_values[ROTATE_VAR_N], rot->var_values[ROTATE_VAR_T], rot->angle/M_PI);
qDebug()<<"rotate_filter_frame n:"<<rot->var_values[ROTATE_VAR_N]
<<" time:"<< rot->var_values[ROTATE_VAR_T] <<" angle(/PI): "<<(rot->angle/M_PI);
angle_int = res * FIXP * 16;
s = int_sin(angle_int);
c = int_sin(angle_int + INT_PI/2);
/* fill background */
if (rot->fillcolor_enable)
ff_fill_rectangle(&rot->draw, &rot->color, out->data, out->linesize,
0, 0, out->width, out->height);
for (plane = 0; plane < rot->nb_planes; plane++) {
int hsub = plane == 1 || plane == 2 ? rot->hsub : 0;
int vsub = plane == 1 || plane == 2 ? rot->vsub : 0;
const int outw = AV_CEIL_RSHIFT(out->width, hsub);
const int outh = AV_CEIL_RSHIFT(out->height, vsub);
#if 0
ThreadData td = { .in = in, .out = out,
.inw = AV_CEIL_RSHIFT(in->width, hsub),
.inh = AV_CEIL_RSHIFT(in->height, vsub),
.outh = outh, .outw = outw,
.xi = -(outw-1) * c / 2, .yi = (outw-1) * s / 2,
.xprime = -(outh-1) * s / 2,
.yprime = -(outh-1) * c / 2,
.plane = plane, .c = c, .s = s };
#endif
ThreadData td;
td.in = in; td.out = out;
td.inw = AV_CEIL_RSHIFT(in->width, hsub);
td.inh = AV_CEIL_RSHIFT(in->height, vsub);
td.outh = outh; td.outw = outw;
td.xi = -(outw-1) * c / 2; td.yi = (outw-1) * s / 2;
td.xprime = -(outh-1) * s / 2;
td.yprime = -(outh-1) * c / 2;
td.plane = plane; td.c = c; td.s = s;
filter_slice(rot, &td, 0, 1);
}
return 0;
}
int rotate_config_props(RotContext *rot, AVFrame *in)
{
const AVPixFmtDescriptor *pixdesc = av_pix_fmt_desc_get((AVPixelFormat)in->format);
int ret;
double res;
char *expr;
qDebug()<<"rotate_config_props foramt: "<<in->format;
ff_draw_init(&rot->draw, (AVPixelFormat)in->format, 0);
ff_draw_color(&rot->draw, &rot->color, rot->fillcolor);
rot->hsub = pixdesc->log2_chroma_w;
rot->vsub = pixdesc->log2_chroma_h;
if (pixdesc->comp[0].depth == 8)
rot->interpolate_bilinear = interpolate_bilinear8;
else
rot->interpolate_bilinear = interpolate_bilinear16;
rot->var_values[ROTATE_VAR_IN_W] = rot->var_values[ROTATE_VAR_IW] = in->width;
rot->var_values[ROTATE_VAR_IN_H] = rot->var_values[ROTATE_VAR_IH] = in->height;
rot->var_values[ROTATE_VAR_HSUB] = 1<<rot->hsub;
rot->var_values[ROTATE_VAR_VSUB] = 1<<rot->vsub;
rot->var_values[ROTATE_VAR_N] = NAN;
rot->var_values[ROTATE_VAR_T] = NAN;
rot->var_values[ROTATE_VAR_OUT_W] = rot->var_values[ROTATE_VAR_OW] = NAN;
rot->var_values[ROTATE_VAR_OUT_H] = rot->var_values[ROTATE_VAR_OH] = NAN;
av_expr_free(rot->angle_expr);
rot->angle_expr = NULL;
if ((ret = av_expr_parse(&rot->angle_expr, expr = rot->angle_expr_str, var_names,
func1_names, func1, NULL, NULL, 0, NULL)) < 0) {
av_log(NULL, AV_LOG_ERROR,
"Error occurred parsing angle expression '%s'\n", rot->angle_expr_str);
return ret;
}
#define SET_SIZE_EXPR(name, opt_name) do { \
ret = av_expr_parse_and_eval(&res, expr = rot->name##_expr_str, \
var_names, rot->var_values, \
func1_names, func1, NULL, NULL, rot, 0, NULL); \
if (ret < 0 || isnan(res) || isinf(res) || res <= 0) { \
av_log(NULL, AV_LOG_ERROR, \
"Error parsing or evaluating expression for option %s: " \
"invalid expression '%s' or non-positive or indefinite value %f\n", \
opt_name, expr, res); \
return ret; \
} \
} while (0)
/* evaluate width and height */
av_expr_parse_and_eval(&res, expr = rot->outw_expr_str, var_names, rot->var_values,
func1_names, func1, NULL, NULL, rot, 0, NULL);
rot->var_values[ROTATE_VAR_OUT_W] = rot->var_values[ROTATE_VAR_OW] = res;
rot->outw = res + 0.5;
SET_SIZE_EXPR(outh, "out_h");
rot->var_values[ROTATE_VAR_OUT_H] = rot->var_values[ROTATE_VAR_OH] = res;
rot->outh = res + 0.5;
/* evaluate the width again, as it may depend on the evaluated output height */
SET_SIZE_EXPR(outw, "out_w");
rot->var_values[ROTATE_VAR_OUT_W] = rot->var_values[ROTATE_VAR_OW] = res;
rot->outw = res + 0.5;
/* compute number of planes */
rot->nb_planes = av_pix_fmt_count_planes((AVPixelFormat)in->format);
////out->width = rot->outw;
////outlink->h = rot->outh;
return 0;
}
