Commit ea4c3cc1 authored by Alessio Igor Bogani's avatar Alessio Igor Bogani
Browse files

Bunch of fixes

parent b1a76951
......@@ -631,37 +631,50 @@ static void disable_pru(int drv)
}
}
double calc_setpoint(double Setpoint, float a0, float a1, float a2, float a3,
float a4, float a5) {
int i;
double calc_setpoint(double Setpoint, float a0, float a1, float a2,
float a3, float a4, float a5)
{
int i, n;
double a[6] = { Setpoint - a0, a1, -a2, a3, -a4, a5 };
double z[10];
gsl_poly_complex_workspace* w = gsl_poly_complex_workspace_alloc(6);
gsl_poly_complex_solve(a, 6, w, z);
gsl_poly_complex_workspace_free(w);
if (a5 != 0) n = 6;
else if (a4 != 0) n = 5;
else if (a3 != 0) n = 4;
else if (a2 != 0) n = 3;
else if (a1 != 0) n = 2;
else n = 1;
if (n > 1) {
gsl_poly_complex_workspace* w = gsl_poly_complex_workspace_alloc(n);
gsl_poly_complex_solve(a, n, w, z);
gsl_poly_complex_workspace_free(w);
#ifndef NDEBUG
bool found = false;
bool found = false;
#endif
for (i = 0; i < 5; i++)
{
if (z[2*i+1] == 0.0) {
for (i = 0; i < 5; i++)
{
if (z[2*i+1] == 0.0) {
#ifndef NDEBUG
found = true;
found = true;
#endif
// return floor(z[2*i]);
return -z[2*i];
// return floor(z[2*i]);
return -z[2*i];
}
}
}
assert(found);
assert(found);
} else
return Setpoint - a0;
return 0; /* never get here */
}
double calc_output(double OCnorm, float a0, float a1, float a2, float a3,
float a4, float a5) {
double OutputCurrent = (a0 + (a1*OCnorm) + (a2*pow(OCnorm,2))
+ (a3*pow(OCnorm,3)) + (a4*pow(OCnorm, 4))
double calc_output(double OCnorm, float a0, float a1, float a2,
float a3, float a4, float a5)
{
double OutputCurrent = (-a0 + (a1*OCnorm) + (-a2*pow(OCnorm,2))
+ (a3*pow(OCnorm,3)) + (-a4*pow(OCnorm, 4))
+ (a5*pow(OCnorm, 5)));
return OutputCurrent;
}
......@@ -779,7 +792,7 @@ int main(int argc, char *argv[])
break;
case CONFIGFILE:
case CONFIG:
if (ioctl(drv, A2720_GET_CONFIG, (unsigned char*)&config) != 0) {
if (ioctl(drv, A2720_GET_CONFIG, &config) != 0) {
perror("Unable to retrieve configuration");
close(drv);
exit(EXIT_FAILURE);
......@@ -787,15 +800,25 @@ int main(int argc, char *argv[])
if (control_action == WRITE) { // ON COMMAND
config.control = control_value;
config.output_current_setpoint = calc_setpoint(0, a0_value, a1_value, a2_value, a3_value, a4_value, a5_value); // set variable for setting to 0 A the output current
// set variable for setting to 0 A the output current
config.output_current_setpoint = calc_setpoint(0, config.a0_coefficient_scaling,
config.a1_coefficient_scaling, config.a2_coefficient_scaling,
config.a3_coefficient_scaling, config.a4_coefficient_scaling,
config.a5_coefficient_scaling);
} else { /* control_action == NOTHING */ }
if (setpoint_action == READ || print_all_config) {
printf("SetPoint:\t\t\t%+-8.5f\tA\nSetPoint_calc:\t\t\t%+-8.5f\tA\n",
calc_output(config.output_current_setpoint, a0_value, a1_value, a2_value, a3_value, a4_value, a5_value)
* Norm, config.output_current_setpoint * Norm);
calc_output(config.output_current_setpoint, config.a0_coefficient_scaling,
config.a1_coefficient_scaling, config.a2_coefficient_scaling,
config.a3_coefficient_scaling, config.a4_coefficient_scaling,
config.a5_coefficient_scaling) * Norm,
config.output_current_setpoint * Norm);
} else if (setpoint_action == WRITE) {
config.output_current_setpoint = calc_setpoint(setpoint_value / Norm, a0_value, a1_value, a2_value, a3_value, a4_value, a5_value); // normalized value
config.output_current_setpoint = calc_setpoint(setpoint_value / Norm,
config.a0_coefficient_scaling, config.a1_coefficient_scaling,
config.a2_coefficient_scaling, config.a3_coefficient_scaling,
config.a4_coefficient_scaling, config.a5_coefficient_scaling); // normalized value
printf("SetPoint_calc:\t\t\t\%+-8.5f\tA\n",config.output_current_setpoint * Norm);
} else { /* setpoint_action == NOTHING */ }
......@@ -958,7 +981,8 @@ int main(int argc, char *argv[])
}
break;
case STATUS:
if (ioctl(drv, A2720_GET_STATUS, &status) != 0) {
if (ioctl(drv, A2720_GET_STATUS, &status) != 0 ||
ioctl(drv, A2720_GET_CONFIG, &config) != 0) {
perror("Unable to retrieve status");
close(drv);
exit(EXIT_FAILURE);
......@@ -994,15 +1018,21 @@ int main(int argc, char *argv[])
printf("Earth fault\n");
}
} else { /* iostatus_action == NOTHING */ }
if (setpoint_action == READ) {
#if 0
if (setpoint_action == READ || print_all_status) {
printf("Output current setpoint:\t%.04f\tA\n",
calc_output(config.output_current_setpoint, a0_value, a1_value, a2_value, a3_value, a4_value, a5_value));
calc_output(config.output_current_setpoint, config.a0_coefficient_scaling,
config.a1_coefficient_scaling, config.a2_coefficient_scaling,
config.a3_coefficient_scaling, config.a4_coefficient_scaling,
config.a5_coefficient_scaling));
} else { /* setpoint_action == NOTHING */ }
#endif
if (output_action == READ || print_all_status) {
printf("Output Current:\t\t\t%+-8.5f\tA\nOutput current_ADC:\t\t%+-8.5f\tA\n",
calc_output(status.output_current, a0_value, a1_value, a2_value, a3_value, a4_value, a5_value) * Norm,
calc_output(status.output_current, config.a0_coefficient_scaling,
config.a1_coefficient_scaling, config.a2_coefficient_scaling,
config.a3_coefficient_scaling, config.a4_coefficient_scaling,
config.a5_coefficient_scaling) * Norm,
status.output_current * Norm);
} else { /* output_action == NOTHING */ }
......@@ -1034,7 +1064,7 @@ int main(int argc, char *argv[])
printf("Heatsink temperature:\t\t%2.1f\t\tC\n", heatsink_a0 + (heatsink_a1 * (float)status.heatsink_temperature));
} else { /* heatsink_action == NOTHING */ }
if (print_all_status) {
if (print_all_status) {
FILE *file_ptr;
file_ptr = fopen("/sys/class/gpio/gpio49/value", "r");
if (! file_ptr) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment