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;
}
Beispiel #2
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
  }
}
Beispiel #3
0
////////////////////////////////////////////////////////////////////////////////////
// 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);
    }
}
Beispiel #5
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
    }
}
Beispiel #6
0
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);
}
Beispiel #8
0
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;
}
Beispiel #9
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
}
Beispiel #11
0
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;
}
Beispiel #12
0
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;
}
Beispiel #14
0
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();
}