int rescorer_main(int argc, char **argv)
{
flow_poster_prob_calc aa;
unsigned char ref[10]={0,1,2,1,0,3,1,2,1,0};
int flow[10] = {1,3,3,1,1,1,2,2,1,1};
aa.setRef(10, ref, flow);
unsigned char f[10] = {0,1,2,1,0,3,1,2,1,0};
int fm[10] = {100,300,300,100,100,100,200,200,100,100};
double x = aa.calc_post_prob(10, f, fm);
unsigned char f1[21] = {0, 1, 2,3,0, 1,2, 3, 0,1,2, 3,0, 1, 2, 3,0, 1, 2,3, 0};
int fm1[21] = {100,300,300,3,3,100,4,11,100,1,1,100,1,200,200,1,1,100,1,1,100};
x = aa.calc_post_prob(21, f1, fm1);
unsigned char f2[25] = {0, 1, 2, 3, 0, 1, 2, 3,0, 1,2, 3, 0,1,2, 3,0, 1, 2, 3,0, 1, 2,3, 0};
int fm2[25] = {100,300,0, 0, 100, 0, 300,3,3,100,4,11,100,1,1,100,1,200,200,1,1,100,1,1,100};
x = aa.calc_post_prob(25, f2, fm2);
unsigned char f3[21] = {0, 1, 2,3,0, 1, 2, 3, 0,1,2, 3,0, 1, 2, 3,0, 1, 2,3, 0};
int fm3[21] = {100,300,300,3,100,100,4,11,100,1,1,100,1,200,200,1,1,100,1,1,100};
x = aa.calc_post_prob(21, f3, fm3);
unsigned char ref1[11]={0,1,0,2,1,0,3,1,2,1,0};
int flow1[11] = {1,3,1,3,1,1,1,2,2,1,1};
aa.setRef(11, ref1, flow1);
x = aa.calc_post_prob(25, f2, fm2);
unsigned char ref2[11]={0,1,2,0,1,0,3,1,2,1,0};
int flow2[11] = {1,3,3,1,1,1,1,2,2,1,1};
aa.setRef(11, ref2, flow2);
x = aa.calc_post_prob(21, f3, fm3);
/*
unsigned char ref4[4]={0,2,0,3};
int flow4[4] = {1,1,1,1};
unsigned char ref5[5]={0,2,1,0,3};
int flow5[5] = {1,1,1,1,1};
unsigned char ref6[5]={0,2,3,0,3};
int flow6[5] = {1,1,1,1,1};
unsigned char f5[12] = {0,1,2,3,0,1,2,3,0,1,2,3};
int fm5[12] = {100,0,100,0,0,100,0,0,100,0,0,100};
unsigned char f6[8] = {0,1,2,3,0,1,2,3};
int fm6[8]={100,0,100,100,100,0,0,100};
aa.setRef(4, ref4, flow4);
x = aa.calc_post_prob(12,f5,fm5);
printf("example1 base %f\n", (float) x);
x = aa.calc_post_prob(8, f6,fm6);
printf("example2 base %f\n", (float) x);
aa.setRef(5, ref5, flow5);
x = aa.calc_post_prob(12,f5,fm5);
printf("example1 alter %f\n", (float) x);
x = aa.calc_post_prob(8, f6,fm6);
printf("ref5 against f6 %f\n", (float) x);
aa.setRef(5, ref6, flow6);
x = aa.calc_post_prob(8, f6,fm6);
printf("example2 alter %f\n", (float) x);
*/
flow_list *hy = new flow_list(20), *rs = new flow_list();
/*
hy->add_list(ref4, flow4, 4, 1.0);
hy->add_list(ref5, flow5, 5, 0.001);
rs->add_list(f5, fm5, 12, 1.0);
rs->add_list(f6,fm6, 8, 1.0);
aa.best_hyp(hy, rs);
*/
if (argc < 4) {
fprintf(stderr, "%s ref_file devfile flow_order_string output_vcffile [input_vcffile] [-log logfile] [-min_score bayscore_cutoff] [-neighbor ##] [-hotspot_file filename]", argv[0]);
exit(1);
}
FastaFile *fafile = new FastaFile(SEQTYPE_NT);
if (!fafile->Open(argv[1],"r"))
return 0;
FastaSeq faseq;
if (!fafile->Read(faseq))
{
fatal("cannot open \n");
}
const char *s = faseq.Sequence();
FILE *fp = ckopen(argv[2], "r");
char *line = new char[sizeofline], *last_line = new char[sizeofline];
last_line[0] = 0;
expand_flow(total_flow_order, 5000, argv[3]);
outp = ckopen(argv[4], "w");
int print_header = 1;
int out_header = 1;
if (argc > 5) {
int i;
for (i = 5; i < argc; i++) {
if (argv[i][0] == '-') {
if (strcmp(argv[i]+1, "log") == 0) {
logfile = ckopen(argv[i+1], "w");
} else if (strcmp(argv[i]+1, "debug")==0|| strcmp(argv[i]+1, "d")==0) {
debug = atoi(argv[i+1]);
} else if (strcmp(argv[i]+1, "min_score")==0) {
bscore_cut = atof(argv[i+1]);
} else if (strcmp(argv[i]+1, "neighbor")==0) {
neighbor = atoi(argv[i+1]);
} else if (strcmp(argv[i]+1, "hotspot_file") == 0) {
hotfile = argv[i+1];
} else {
fprintf(stderr, "wrong option %s\n", argv[i]);
exit(1);
}
i++;
} else {
FILE *input_dev = ckopen(argv[i], "r");
print_header = 0;
while (fgets(line, sizeofline, input_dev)) {
if (line[0] == '#') {
if (out_header && line[1] != '#') {
fprintf(outp, "##FILTER=<ID=Bayesian_Score,Description=\"Using Bayesian model to re-evaluate the quality of the variant prediction.\">\n");
out_header = 0;
}
fprintf(outp, "%s", line);
} else break;
}
}
}
}
clearCurSeq(".");
int diff, coverage;
int need_check = 0;
float vh;
while (fgets(line, sizeofline, fp)) {
if (line[0] == '#') {
if (print_header) fprintf(outp, "%s", line);
continue;
}
if (line[0] == '\n') break;
if (out_header) {
out_header = 0;
fprintf(outp, "##FILTER=<ID=Bayesian_Score,Number=1,Type=float,Description=\"Using Bayesian model to re-evaluate the quality of the variant prediction.\">\n");
}
char sid[100];
sscanf(line, "%s", sid);
//fprintf(stderr, "seq %s\n", sid);
if (need_check && (x = need_combine(last_line, line))) {
if (x == 1) {
hy->reset();
diff = set_hyp(line, hy, s);
coverage = get_cov(line);
combine_str(last_line, line);
chomp(last_line);
}
need_check = 0;
} else {
if (last_line[0] != 0) {
//fprintf(outp, "%s", last_line);
vh = vhscore(last_line);
aa.best_hyp(hy, rs, coverage, vh, last_line);
}
while (strcmp(sid, faseq.Label())!=0) {
if (debug) {printf("INFO: before call clearCurSeq: %s %s\n", faseq.Label(), sid);}
clearCurSeq(faseq.Label());
if (!fafile->Read(faseq)) {
fatal("cannot open \n");
}
//fprintf(stderr, "from reference %s\n", faseq.Label());
s = faseq.Sequence();
}
hy->reset();
rs->reset();
strcpy(last_line, line);
chomp(last_line);
diff = set_hyp(line, hy, s);
coverage = get_cov(line);
need_check = 1;
}
//printf("coverage=%d\n", coverage);
fgets(line, sizeofline, fp); // definition line skip
while (fgets(line, sizeofline, fp)) {
if (line[0] == '\n') break;
if (line[0] == 'I') continue;
if (!add_read(fp, line, rs, diff)) fatal("faill to read a line\n");
}
//aa.best_hyp(hy, rs, coverage);
}
//fprintf(outp, "%s", last_line);
vh = vhscore(last_line);
aa.best_hyp(hy, rs, coverage, vh, last_line);
if (num_error > 0) {
fprintf(stderr, "Detecting %d reads with wrong flow information that can not be resolved by rescorer. Please check log file.\n", num_error);
}
}
void vertical_ctrl_module_run(bool in_flight)
{
int i;
float lp_height; // low-pass height
float div_factor; // factor that maps divergence in pixels as received from vision to /frame
// ensure dt >= 0
if (dt < 0) { dt = 0.0f; }
// get delta time, dt, to scale the divergence measurements correctly when using "simulated" vision:
struct timespec spec;
clock_gettime(CLOCK_REALTIME, &spec);
long new_time = spec.tv_nsec / 1.0E6;
long delta_t = new_time - previous_time;
dt += ((float)delta_t) / 1000.0f;
if (dt > 10.0f) {
dt = 0.0f;
return;
}
previous_time = new_time;
if (!in_flight) {
// When not flying and in mode module:
// Reset integrators
reset_all_vars();
} else {
/***********
* VISION
***********/
if (of_landing_ctrl.VISION_METHOD == 0) {
// SIMULATED DIVERGENCE:
// USE OPTITRACK HEIGHT
of_landing_ctrl.agl = (float) gps.lla_pos.alt / 1000.0f;
// else we get an immediate jump in divergence when switching on.
if (of_landing_ctrl.agl_lp < 1E-5 || ind_hist == 0) {
of_landing_ctrl.agl_lp = of_landing_ctrl.agl;
}
if (fabs(of_landing_ctrl.agl - of_landing_ctrl.agl_lp) > 1.0f) {
// ignore outliers:
of_landing_ctrl.agl = of_landing_ctrl.agl_lp;
}
// calculate the new low-pass height and the velocity
lp_height = of_landing_ctrl.agl_lp * of_landing_ctrl.lp_factor + of_landing_ctrl.agl * (1.0f - of_landing_ctrl.lp_factor);
// only calculate velocity and divergence if dt is large enough:
if (dt > 0.0001f) {
of_landing_ctrl.vel = (lp_height - of_landing_ctrl.agl_lp) / dt;
of_landing_ctrl.agl_lp = lp_height;
// calculate the fake divergence:
if (of_landing_ctrl.agl_lp > 0.0001f) {
divergence = of_landing_ctrl.vel / of_landing_ctrl.agl_lp;
divergence_vision_dt = (divergence_vision / dt);
if (fabs(divergence_vision_dt) > 1E-5) {
div_factor = divergence / divergence_vision_dt;
}
} else {
divergence = 1000.0f;
// perform no control with this value (keeping thrust the same)
return;
}
// reset dt:
dt = 0.0f;
}
} else {
// USE REAL VISION OUTPUTS:
if (vision_message_nr != previous_message_nr && dt > 1E-5 && ind_hist > 1) {
div_factor = -1.28f; // magic number comprising field of view etc.
float new_divergence = (divergence_vision * div_factor) / dt;
if (fabs(new_divergence - divergence) > 0.20) {
if (new_divergence < divergence) { new_divergence = divergence - 0.10f; }
else { new_divergence = divergence + 0.10f; }
}
// low-pass filter the divergence:
divergence = divergence * of_landing_ctrl.lp_factor + (new_divergence * (1.0f - of_landing_ctrl.lp_factor));
previous_message_nr = vision_message_nr;
dt = 0.0f;
} else {
// after re-entering the module, the divergence should be equal to the set point:
if (ind_hist <= 1) {
divergence = of_landing_ctrl.divergence_setpoint;
for (i = 0; i < COV_WINDOW_SIZE; i++) {
thrust_history[i] = 0;
divergence_history[i] = 0;
}
ind_hist++;
dt = 0.0f;
int32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ;
stabilization_cmd[COMMAND_THRUST] = nominal_throttle;
}
// else: do nothing, let dt increment
return;
}
}
/***********
* CONTROL
***********/
int32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ; \
// landing indicates whether the drone is already performing a final landing procedure (flare):
if (!landing) {
if (of_landing_ctrl.CONTROL_METHOD == 0) {
// fixed gain control, cov_limit for landing:
// use the divergence for control:
float err = of_landing_ctrl.divergence_setpoint - divergence;
int32_t thrust = nominal_throttle + of_landing_ctrl.pgain * err * MAX_PPRZ + of_landing_ctrl.igain * of_landing_ctrl.sum_err * MAX_PPRZ;
// make sure the p gain is logged:
pstate = of_landing_ctrl.pgain;
pused = pstate;
// bound thrust:
Bound(thrust, 0.8 * nominal_throttle, 0.75 * MAX_PPRZ);
// histories and cov detection:
normalized_thrust = (float)(thrust / (MAX_PPRZ / 100));
thrust_history[ind_hist % COV_WINDOW_SIZE] = normalized_thrust;
divergence_history[ind_hist % COV_WINDOW_SIZE] = divergence;
int ind_past = (ind_hist % COV_WINDOW_SIZE) - of_landing_ctrl.delay_steps;
while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
float past_divergence = divergence_history[ind_past];
past_divergence_history[ind_hist % COV_WINDOW_SIZE] = past_divergence;
ind_hist++;
// determine the covariance for landing detection:
if (of_landing_ctrl.COV_METHOD == 0) {
cov_div = get_cov(thrust_history, divergence_history, COV_WINDOW_SIZE);
} else {
cov_div = get_cov(past_divergence_history, divergence_history, COV_WINDOW_SIZE);
}
if (ind_hist >= COV_WINDOW_SIZE && fabs(cov_div) > of_landing_ctrl.cov_limit) {
// land by setting 90% nominal thrust:
landing = 1;
thrust = 0.90 * nominal_throttle;
}
stabilization_cmd[COMMAND_THRUST] = thrust;
of_landing_ctrl.sum_err += err;
} else {
// ADAPTIVE GAIN CONTROL:
// adapt the gains according to the error in covariance:
float error_cov = of_landing_ctrl.cov_set_point - cov_div;
// limit the error_cov, which could else become very large:
if (error_cov > fabs(of_landing_ctrl.cov_set_point)) { error_cov = fabs(of_landing_ctrl.cov_set_point); }
pstate -= (of_landing_ctrl.igain_adaptive * pstate) * error_cov;
if (pstate < MINIMUM_GAIN) { pstate = MINIMUM_GAIN; }
// regulate the divergence:
float err = of_landing_ctrl.divergence_setpoint - divergence;
pused = pstate - (of_landing_ctrl.pgain_adaptive * pstate) * error_cov;
// make sure pused does not become too small, nor grows too fast:
if (pused < MINIMUM_GAIN) { pused = MINIMUM_GAIN; }
if (of_landing_ctrl.COV_METHOD == 1 && error_cov > 0.001) {
pused = 0.5 * pused;
}
// set thrust:
int32_t thrust = nominal_throttle + pused * err * MAX_PPRZ + of_landing_ctrl.igain * of_landing_ctrl.sum_err * MAX_PPRZ;
// histories and cov detection:
normalized_thrust = (float)(thrust / (MAX_PPRZ / 100));
thrust_history[ind_hist % COV_WINDOW_SIZE] = normalized_thrust;
divergence_history[ind_hist % COV_WINDOW_SIZE] = divergence;
int ind_past = (ind_hist % COV_WINDOW_SIZE) - of_landing_ctrl.delay_steps;
while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
float past_divergence = divergence_history[ind_past];
past_divergence_history[ind_hist % COV_WINDOW_SIZE] = 100.0f * past_divergence;
ind_hist++;
// only take covariance into account if there are enough samples in the histories:
if (ind_hist >= COV_WINDOW_SIZE) {
if (of_landing_ctrl.COV_METHOD == 0) {
cov_div = get_cov(thrust_history, divergence_history, COV_WINDOW_SIZE);
} else {
cov_div = get_cov(past_divergence_history, divergence_history, COV_WINDOW_SIZE);
}
} else {
cov_div = of_landing_ctrl.cov_set_point;
}
// TODO: could put a landing condition here based on pstate (if too low)
// bound thrust:
Bound(thrust, 0.8 * nominal_throttle, 0.75 * MAX_PPRZ); // was 0.6 0.9
stabilization_cmd[COMMAND_THRUST] = thrust;
of_landing_ctrl.sum_err += err;
}
} else {
// land with 90% nominal thrust:
int32_t thrust = 0.90 * nominal_throttle;
Bound(thrust, 0.6 * nominal_throttle, 0.9 * MAX_PPRZ);
stabilization_cmd[COMMAND_THRUST] = thrust;
}
}
}
