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