double pulse_newton_sim_voc(struct device *in) { printf_log("Looking for Voc\n"); double C = in->C; double clamp = 0.1; double step = 0.01; double e0; double e1; double i0; double i1; double deriv; double Rdrain = pulse_config.pulse_Rload + in->Rcontact; solve_all(in); i0 = get_I(in); e0 = fabs(i0 + in->Vapplied * (1.0 / in->Rshunt - 1.0 / Rdrain)); in->Vapplied += step; solve_all(in); i1 = get_I(in); e1 = fabs(i1 + in->Vapplied * (1.0 / in->Rshunt - 1.0 / Rdrain)); deriv = (e1 - e0) / step; step = -e1 / deriv; step = step / (1.0 + fabs(step / clamp)); in->Vapplied += step; int count = 0; int max = 200; do { e0 = e1; solve_all(in); i1 = get_I(in); e1 = fabs(i1 + in->Vapplied * (1.0 / in->Rshunt - 1.0 / Rdrain)); deriv = (e1 - e0) / step; step = -e1 / deriv; step = step / (1.0 + fabs(step / clamp)); in->Vapplied += step; if (get_dump_status(dump_print_text) == TRUE) { printf_log ("%d pulse voc find Voc Vapplied=%lf step=%le error=%le\n", count, in->Vapplied, step, e1); } if (count > max) break; count++; } while (e1 > 1e-12); double ret = in->Vapplied - C * (i1 - in->Ilast) / in->dt; return ret; }
//////////////////////////////////////////////////////////////////////////////////// // Calculate the desired nav_lat and nav_lon for distance flying such as RTH // static void GPS_calc_nav_rate(uint16_t max_speed) { float trig[2]; uint8_t axis; float temp; // push us towards the original track GPS_update_crosstrack(); // nav_bearing includes crosstrack temp = (9000l - nav_bearing) * RADX100; trig[_X] = cos(temp); trig[_Y] = sin(temp); for (axis=0;axis<2;axis++) { rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis]; rate_error[axis] = constrain(rate_error[axis], -1000, 1000); // P + I + D nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM); nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); poshold_ratePID[axis].integrator = navPID[axis].integrator; } }
//////////////////////////////////////////////////////////////////////////////////// // Calculate nav_lat and nav_lon from the x and y error and the speed // static void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; uint8_t axis; for (axis=0;axis<2;axis++) { target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lat/lon error target_speed = constrain(target_speed,-100,100); // Constrain the target speed in poshold mode to 1m/s it helps avoid runaways.. rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = constrain(d, -2000, 2000); // get rid of noise if(fabs(actual_speed[axis]) < 50) d = 0; nav[axis] +=d; nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); navPID[axis].integrator = poshold_ratePID[axis].integrator; } }
//////////////////////////////////////////////////////////////////////////////////// // Crashpilot NEW PH Logic // #define GPS_X 1 // #define GPS_Y 0 // #define LON 1 // #define LAT 0 // VelEast; // 1 // VelNorth; // 0 // 0 is NICK part // 1 is ROLL part // actual_speed[GPS_Y] = VelNorth; void GPS_calc_posholdCrashpilot(bool overspeed) { uint8_t axis; float p, i, d, rate_error, AbsPosErrorToVel, tmp0, tmp1; float maxbank100new; for (axis = 0; axis < 2; axis++) { maxbank100new = maxbank100; if (overspeed) { tmp1 = abs(ACC_speed[axis]); if (tmp1 == 0) tmp1 = 1.0f; tmp0 = (float)cfg.gps_ph_settlespeed / tmp1; tmp1 = (float)cfg.gps_ph_minbrakepercent * 0.01f; // this is the minimal break percentage tmp0 = constrain(sqrt(tmp0), tmp1, 1.0f); // 1 - 100% maxbank100new = (float)maxbankbrake100 * tmp0; } tmp1 = LocError[axis]; if (abs(tmp1) < cfg.gps_ph_abstub && cfg.gps_ph_abstub != 0) // Keep linear Stuff beyond x cm { if (tmp1 < 0) tmp0 = -GpsPhAbsTub; // Do flatter response below x cm else tmp0 = GpsPhAbsTub; tmp1 = tmp1 * tmp1 * tmp0; // Get a peace around current position } AbsPosErrorToVel = get_P(tmp1, &posholdPID_PARAM); // Do absolute position error here, that means transform positionerror to a velocityerror rate_error = AbsPosErrorToVel - ACC_speed[axis]; // Calculate Rate Error for actual axis rate_error = constrain(rate_error, -1000, 1000); // +- 10m/s p = get_P(rate_error, &poshold_ratePID_PARAM); i = get_I(AbsPosErrorToVel, &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P d = get_D(rate_error , &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P nav[axis] = constrain(p + i + d, -maxbank100new, maxbank100new); } }
//////////////////////////////////////////////////////////////////////////////////// // Calculate nav_lat and nav_lon from the x and y error and the speed // static void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; int axis; for (axis = 0; axis < 2; axis++) { target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lon error rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) + get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = constrain(d, -2000, 2000); // get rid of noise #if defined(GPS_LOW_SPEED_D_FILTER) if (abs(actual_speed[axis]) < 50) d = 0; #endif nav[axis] += d; nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); navPID[axis].integrator = poshold_ratePID[axis].integrator; } }
void GPS_calc_posholdCrashpilot(bool overspeed) { uint8_t axis; float rate_error, tilt, tmp0, tmp1; float maxbank100new; for (axis = 0; axis < 2; axis++) { maxbank100new = (float)maxbank100; if (overspeed) { tmp1 = fabs(ACC_speed[axis]); if (!tmp1) tmp1 = 1.0f; tmp0 = (float)cfg.gps_ph_settlespeed / tmp1; tmp1 = (float)cfg.gps_ph_minbrakepercent * 0.01f; // this is the minimal break percentage tmp0 = constrain(sqrtf(tmp0), tmp1, 1.0f); // 1 - 100% maxbank100new = (float)maxbankbrake100 * tmp0; } rate_error = get_P(LocError[axis], &posholdPID_PARAM); // Do absolute position error here, that means transform positionerror to a velocityerror rate_error += get_I(LocError[axis], &ACCDeltaTimeINS, &posholdPID[axis], &posholdPID_PARAM); rate_error = constrain(rate_error - ACC_speed[axis], -1000, 1000); // +- 10m/s; // Calculate velocity Error for actual axis tilt = get_P(rate_error, &poshold_ratePID_PARAM); tilt += get_D(rate_error, &ACCDeltaTimeINS, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P nav[axis] = constrain(tilt, -maxbank100new, maxbank100new); } }
void GaussianProcessInterpolation::compute_OmiIm() { IMP_Eigen::VectorXd I(get_I()); IMP_Eigen::VectorXd m(get_m()); IMP_Eigen::MatrixXd Omi(get_Omi()); IMP_LOG_TERSE("OmiIm "); OmiIm_ = ldlt_.solve(I - m); IMP_LOG_TERSE(std::endl); }
gdouble newton_sim_voc_fast(struct simulation *sim,struct device *in,int do_LC) { gdouble Vapplied=0.0; gdouble Vapplied_last=0.0; Vapplied=contact_get_voltage(sim,in,0); Vapplied_last=contact_get_voltage_last(sim,in,0); newton_sim_voc(sim,in); return get_I(in)+in->C*(Vapplied-Vapplied_last)+Vapplied/in->Rshunt; }
//////////////////////////////////////////////////////////////////////////////////// // Calculate the desired nav_lat and nav_lon for distance flying such as RTH void GPS_calc_nav_rate(uint16_t max_speed) { float trig[2], rate_error; float temp, tiltcomp, trgtspeed; uint8_t axis; int32_t crosstrack_error; if ((abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) && cfg.nav_ctrkgain != 0)// If we are too far off or too close we don't do track following { temp = (float)(target_bearing - original_target_bearing) * RADX100; crosstrack_error = sinf(temp) * (float)wp_distance * cfg.nav_ctrkgain; // Meters we are off track line nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); while (nav_bearing < 0) nav_bearing += 36000; // nav_bearing = wrap_36000(nav_bearing); while (nav_bearing >= 36000) nav_bearing -= 36000; } else nav_bearing = target_bearing; temp = (float)(9000l - nav_bearing) * RADX100; // nav_bearing and maybe crosstrack trig[GPS_X] = cosf(temp); trig[GPS_Y] = sinf(temp); for (axis = 0; axis < 2; axis++) { trgtspeed = (trig[axis] * (float)max_speed); // Target speed rate_error = trgtspeed - MIX_speed[axis]; // Since my INS Stuff is shit, reduce ACC influence to 50% anyway better than leadfilter rate_error = constrain(rate_error, -1000, 1000); nav[axis] = get_P(rate_error, &navPID_PARAM) + // P + I + D get_I(rate_error, &ACCDeltaTimeINS, &navPID[axis], &navPID_PARAM) + get_D(rate_error, &ACCDeltaTimeINS, &navPID[axis], &navPID_PARAM); if (cfg.nav_tiltcomp) // Do the apm 2.9.1 magic tiltcompensation { tiltcomp = trgtspeed * trgtspeed * ((float)cfg.nav_tiltcomp * 0.0001f); if(trgtspeed < 0) tiltcomp = -tiltcomp; } else tiltcomp = 0; nav[axis] = nav[axis] + tiltcomp; // Constrain is done at the end of all GPS stuff poshold_ratePID[axis].integrator = navPID[axis].integrator; } }
Floats GaussianProcessInterpolation::get_data_mean() const { Floats ret; IMP_Eigen::VectorXd I(get_I()); for (unsigned i = 0; i < M_; i++) ret.push_back(I(i)); return ret; }
gdouble newton_sim_voc(struct simulation *sim, struct device *in) { printf_log(sim,"Looking for Voc\n"); gdouble C=in->C; gdouble clamp=0.1; gdouble step=0.01; gdouble e0; gdouble e1; gdouble i0; gdouble i1; gdouble deriv; gdouble Rdrain=in->Rload+in->Rcontact; gdouble Vapplied=0.0; gdouble Vapplied_last=0.0; Vapplied=contact_get_voltage(sim,in,0); Vapplied_last=contact_get_voltage_last(sim,in,0); solve_all(sim,in); i0=get_I(in); e0=fabs(i0+Vapplied*(1.0/in->Rshunt-1.0/Rdrain)); Vapplied+=step; contact_set_voltage(sim,in,0,Vapplied); solve_all(sim,in); i1=get_I(in); e1=fabs(i1+Vapplied*(1.0/in->Rshunt-1.0/Rdrain)); deriv=(e1-e0)/step; step=-e1/deriv; step=step/(1.0+fabs(step/clamp)); Vapplied+=step; contact_set_voltage(sim,in,0,Vapplied); int count=0; int max=200; do { e0=e1; solve_all(sim,in); i1=get_I(in); e1=fabs(i1+Vapplied*(1.0/in->Rshunt-1.0/Rdrain)); deriv=(e1-e0)/step; step=-e1/deriv; step=step/(1.0+fabs(step/clamp)); Vapplied+=step; contact_set_voltage(sim,in,0,Vapplied); if (get_dump_status(sim,dump_print_text)==TRUE) { printf_log(sim,"%d voc find Voc Vapplied=%Lf step=%Le error=%Le\n",count,Vapplied,step,e1); } if (count>max) break; count++; }while(e1>1e-12); gdouble ret=Vapplied-C*(i1-in->Ilast)/in->dt; return ret; }
int solve_cur(struct device *in) { double error; int ittr = 0; if (get_dump_status(dump_print_newtonerror) == TRUE) { printf("Solve cur\n"); } #ifdef only only_update_thermal = FALSE; #endif //in->enable_back=FALSE; int stop = FALSE; int thermalrun = 0; double check[10]; int cpos = 0; int i = 0; //for (i=0;i<in->ymeshpoints;i++) //{ // printf("Rod ------- nt= %d %le\n",i,in->Gn[i]); //} do { fill_matrix(in); //dump_for_plot(in); //plot_now(in,"plot"); //solver_dump_matrix(in->M,in->N,in->Ti,in->Tj, in->Tx,in->b); //getchar(); if (in->stop == TRUE) { break; } solver(in->M, in->N, in->Ti, in->Tj, in->Tx, in->b); int propper = TRUE; update_solver_vars(in, TRUE); //printf("Going to clamp=%d\n",propper); //solver_dump_matrix(in->M,in->N,in->Ti,in->Tj, in->Tx,in->b); //printf("%d\n"); //getchar(); error = get_cur_error(in); //thermalrun++; if (thermalrun == 40) thermalrun = 0; //update(in); //getchar(); if (get_dump_status(dump_print_newtonerror) == TRUE) { printf("%d Cur error = %e %le I=%le", ittr, error, in->Vapplied, get_I(in)); printf("\n"); } in->last_error = error; in->last_ittr = ittr; ittr++; if (get_dump_status(dump_write_converge) == TRUE) { in->converge = fopena(in->outputpath, "converge.dat", "a"); fprintf(in->converge, "%e\n", error); fclose(in->converge); } stop = TRUE; if (ittr < in->max_electrical_itt) { if (error > in->min_cur_error) { stop = FALSE; } } if (ittr < in->newton_min_itt) { stop = FALSE; } if (in->newton_clever_exit == TRUE) { check[cpos] = error; cpos++; if (cpos > 10) { cpos = 0; } if (ittr >= in->newton_min_itt) { if ((check[0] < error) || (check[1] < error)) { stop = TRUE; } } } } while (stop == FALSE); in->newton_last_ittr = ittr; if (error > 1e-3) { printf_log ("warning: The solver has not converged very well.\n"); } //getchar(); if (get_dump_status(dump_newton) == TRUE) { dump_1d_slice(in, in->outputpath); } //plot_now(in,"plot"); //getchar(); in->odes += in->M; //getchar(); return 0; }
double pulse_newton_sim_voc_fast(struct device *in, int do_LC) { pulse_newton_sim_voc(in); return get_I(in) + in->C * (in->Vapplied - in->Vapplied_last) + in->Vapplied / in->Rshunt; }
void sim_pulse(struct device *in) { struct buffer buf; buffer_init(&buf); struct dynamic_store store; dump_dynamic_init(&store, in); struct istruct out_i; inter_init(&out_i); struct istruct out_v; inter_init(&out_v); struct istruct out_G; inter_init(&out_G); struct istruct lost_charge; inter_init(&lost_charge); char config_file_name[200]; if (find_config_file (config_file_name, in->inputpath, in->simmode, "pulse") != 0) { ewe("%s %s %s\n", _("no pulse config file found"), in->inputpath, in->simmode); } printf("%s\n", config_file_name); pulse_load_config(&pulse_config, in, config_file_name); int number = strextract_int(config_file_name); ntricks_externv_set_load(pulse_config.pulse_Rload); in->go_time = FALSE; in->time = 0.0; time_init(in); //time_load_mesh(in,number); time_load_mesh(in, number); //time_mesh_save(); //getchar(); //struct istruct pulseout; //inter_init(&pulseout); int ittr = 0; int step = 0; in->Psun = time_get_sun(); light_solve_and_update(in, &(in->mylight), time_get_sun(), time_get_laser()); double V = 0.0; if (pulse_config.pulse_sim_mode == pulse_load) { sim_externalv(in, time_get_voltage()); ntricks_externv_newton(in, time_get_voltage(), FALSE); } else if (pulse_config.pulse_sim_mode == pulse_open_circuit) { in->Vapplied = in->Vbi; pulse_newton_sim_voc(in); pulse_newton_sim_voc_fast(in, FALSE); } else { ewe(_("pulse mode not known\n")); } device_timestep(in); in->go_time = TRUE; double extracted_through_contacts = 0.0; double i0 = 0; carrier_count_reset(in); reset_np_save(in); do { in->Psun = time_get_sun(); light_solve_and_update(in, &(in->mylight), time_get_sun(), time_get_laser() + time_get_fs_laser()); dump_dynamic_add_data(&store, in, in->time); if (pulse_config.pulse_sim_mode == pulse_load) { i0 = ntricks_externv_newton(in, time_get_voltage(), TRUE); } else if (pulse_config.pulse_sim_mode == pulse_open_circuit) { V = in->Vapplied; pulse_newton_sim_voc_fast(in, TRUE); } else { ewe(_("pulse mode not known\n")); } if (get_dump_status(dump_print_text) == TRUE) { printf_log("%s=%e %s=%d %.1e ", _("pulse time"), in->time, _("step"), step, in->last_error); printf_log("Vtot=%lf %s = %e mA (%e A/m^2)\n", V, _("current"), get_I(in) / 1e-3, get_J(in)); } ittr++; gui_send_data("pulse"); dump_write_to_disk(in); plot_now(in, "pulse.plot"); inter_append(&out_i, in->time, i0); inter_append(&out_v, in->time, V); inter_append(&out_G, in->time, in->Gn[0]); inter_append(&lost_charge, in->time, extracted_through_contacts - fabs(get_extracted_n(in) + get_extracted_p(in)) / 2.0); device_timestep(in); step++; if (time_run() == FALSE) break; //getchar(); } while (1); struct istruct out_flip; dump_dynamic_save(in->outputpath, &store); dump_dynamic_free(&store); buffer_malloc(&buf); buf.y_mul = 1e3; buf.x_mul = 1e6; strcpy(buf.title, _("Time - current")); strcpy(buf.type, _("xy")); strcpy(buf.x_label, _("Time")); strcpy(buf.y_label, _("Current")); strcpy(buf.x_units, _("us")); strcpy(buf.y_units, _("m")); buf.logscale_x = 0; buf.logscale_y = 0; buffer_add_info(&buf); buffer_add_xy_data(&buf, out_i.x, out_i.data, out_i.len); buffer_dump_path(in->outputpath, "pulse_i.dat", &buf); buffer_free(&buf); inter_copy(&out_flip, &out_i, TRUE); inter_mul(&out_flip, -1.0); buffer_malloc(&buf); buf.y_mul = 1e3; buf.x_mul = 1e6; strcpy(buf.title, _("Time - -current")); strcpy(buf.type, _("xy")); strcpy(buf.x_label, _("Time")); strcpy(buf.y_label, _("-Current")); strcpy(buf.x_units, _("us")); strcpy(buf.y_units, _("mA")); buf.logscale_x = 0; buf.logscale_y = 0; buffer_add_info(&buf); buffer_add_xy_data(&buf, out_flip.x, out_flip.data, out_flip.len); buffer_dump_path(in->outputpath, "pulse_i_pos.dat", &buf); buffer_free(&buf); inter_free(&out_flip); buffer_malloc(&buf); buf.y_mul = 1.0; buf.x_mul = 1e6; strcpy(buf.title, _("Time - Voltage")); strcpy(buf.type, _("xy")); strcpy(buf.x_label, _("Time")); strcpy(buf.y_label, _("Volts")); strcpy(buf.x_units, _("us")); strcpy(buf.y_units, _("Voltage")); buf.logscale_x = 0; buf.logscale_y = 0; buffer_add_info(&buf); buffer_add_xy_data(&buf, out_v.x, out_v.data, out_v.len); buffer_dump_path(in->outputpath, "pulse_v.dat", &buf); buffer_free(&buf); buffer_malloc(&buf); buf.y_mul = 1.0; buf.x_mul = 1e6; strcpy(buf.title, _("Time - Photogeneration rate")); strcpy(buf.type, _("xy")); strcpy(buf.x_label, _("Time")); strcpy(buf.y_label, _("Generation rate")); strcpy(buf.x_units, _("s")); strcpy(buf.y_units, _("m^{-3} s^{-1}")); buf.logscale_x = 0; buf.logscale_y = 0; buffer_add_info(&buf); buffer_add_xy_data(&buf, out_G.x, out_G.data, out_G.len); buffer_dump_path(in->outputpath, "pulse_G.dat", &buf); buffer_free(&buf); //sprintf(outpath,"%s%s",in->outputpath,"pulse_lost_charge.dat"); //inter_save(&lost_charge,outpath); in->go_time = FALSE; inter_free(&out_G); inter_free(&out_i); inter_free(&out_v); time_memory_free(); }