/*
typedef struct bmcdata {
double pv_voltage,cc_voltage, input_voltage, b1_voltage, b2_voltage, system_voltage,logic_voltage;
double pv_current,cc_current,battery_current;
struct didata datain;
struct dodata dataout;
int32_t utc;
}
*/
int get_data_sample(void) {
int i;
for (i = 0; i < 1; i++) { // several samples per reading
bmc.pv_voltage = lp_filter(get_adc_volts(PVV_C), PVV_C, TRUE);
bmc.cc_voltage = lp_filter(get_adc_volts(CCV_C), CCV_C, TRUE);
// bmc.input_voltage = lp_filter(get_adc_volts(INV_C), INV_C, TRUE);
// bmc.b1_voltage = lp_filter(get_adc_volts(B1V_C), B1V_C, TRUE);
// bmc.b2_voltage = lp_filter(get_adc_volts(B2V_C), B2V_C, TRUE);
// bmc.pv_current = lp_filter(get_adc_volts(PVC_C), PVC_C, TRUE);
// bmc.cc_current = lp_filter(get_adc_volts(CCC_C), CCC_C, TRUE);
// bmc.battery_current = lp_filter(get_adc_volts(BAC_C), BAC_C, TRUE);
}
// bmc.system_voltage = get_adc_volts(SYV_C);
// bmc.logic_voltage = get_adc_volts(VD5_C);
bmc.datain.D0 = get_dio_bit(8); // GPIO 0/2
bmc.datain.D1 = get_dio_bit(9); // GPIO 1/3
bmc.datain.D2 = get_dio_bit(0); // read output bit wpi 0
bmc.datain.D3 = get_dio_bit(1); // read output bit wpi 1
bmc.datain.D4 = get_dio_bit(12);
bmc.datain.D5 = get_dio_bit(13);
bmc.datain.D6 = get_dio_bit(15); // GPIO 14
bmc.datain.D7 = get_dio_bit(16); // GPIO 15
put_dio_bit(0, bmc.dataout.D0);
put_dio_bit(1, bmc.dataout.D1);
put_dio_bit(2, bmc.dataout.D2);
put_dio_bit(3, bmc.dataout.D3);
put_dio_bit(4, bmc.dataout.D4);
put_dio_bit(5, bmc.dataout.D5);
put_dio_bit(6, bmc.dataout.D6);
put_dio_bit(7, bmc.dataout.D7);
}
int get_data_sample(void)
{
int i;
static int pv_stable = 0, current_null_reset = FALSE, first_run = TRUE;
if (HAVE_AI) {
bmc.pv_voltage = lp_filter(get_adc_volts(PVV_C, TRUE, RANGE_10) * PVV_GAIN, PVV_C, TRUE); // read PV voltage on DIFF channels
bmc.cm_voltage = lp_filter(get_adc_volts(CMV_C, FALSE, RANGE_5), CMV_C, TRUE); // back to SE channels
bmc.cm_current = lp_filter(get_adc_volts(CMA_C, FALSE, RANGE_5), CMA_C, TRUE);
if (bmc.pv_voltage < 0.0) bmc.pv_voltage = 0.0;
if (bmc.cm_voltage < 0.0) bmc.cm_voltage = 0.0;
if (bmc.cm_current < 0.0) bmc.cm_current = 0.0;
if (first_run) {
first_run = FALSE;
bmc.cm_null = CMA_DEFAULT;
get_adc_volts(PVV_NULL, TRUE, RANGE_10);
}
if (bmc.pv_voltage > 3.0) { // use the default current null if we have not reset the null
if (!current_null_reset) bmc.cm_null = bmc.cm_voltage * CMA_NULL;
pv_stable = 0; // when voltage drops wait a bit until the null update
} else {
if (pv_stable++ > 64) current_null_reset = TRUE; // wait for readings to be stable
if (current_null_reset) {
if (bmc.cm_current > 2.0) bmc.cm_null = bmc.cm_current; // make sure we have something real
current_null_reset = FALSE;
}
}
bmc.cm_amps = lp_filter((bmc.cm_current - bmc.cm_null) / CMA_GAIN, CMC_C, TRUE);
if (bmc.cm_amps < 0.0) bmc.cm_amps = 0.0;
} else {
return -1;
}
// FIXME
if (HAVE_DIO) {
bmc.datain.D0 = get_dio_bit(8);
bmc.datain.D1 = get_dio_bit(9);
bmc.datain.D2 = get_dio_bit(10);
bmc.datain.D3 = get_dio_bit(11);
bmc.datain.D4 = get_dio_bit(12);
bmc.datain.D5 = get_dio_bit(13);
bmc.datain.D6 = get_dio_bit(14);
bmc.datain.D7 = get_dio_bit(15);
put_dio_bit(0, bmc.dataout.D0);
put_dio_bit(1, bmc.dataout.D1);
put_dio_bit(2, bmc.dataout.D2);
put_dio_bit(3, bmc.dataout.D3);
put_dio_bit(4, bmc.dataout.D4);
put_dio_bit(5, bmc.dataout.D5);
put_dio_bit(6, bmc.dataout.D6);
put_dio_bit(7, bmc.dataout.D7);
} else {
return -2;
}
return 0;
}
int get_data_sample(void)
{
int i;
static int pv_stable = 0, first_run = TRUE, set_range;
if (HAVE_AI) {
if (bmc.pv_voltage < 0.5) {
set_range = RANGE_0_512;
gain_adj = ADGAIN2;
} else {
set_range = RANGE_2_048;
gain_adj = ADGAIN1;
}
if (bmc.pv_voltage < 0.0) bmc.pv_voltage = 0.0;
if (first_run) {
if (pv_stable++ > PVV_NULL_TIME) first_run = FALSE;
if (RAW_DATA) {
if (pv_stable > PVV_NULL_TIME_RAW) first_run = FALSE;
}
bmc.pv_voltage_null = lp_filter(get_adc_volts(PVV_NULL, TRUE, set_range), PVV_NULL, FALSE);
;
} else {
if (RAW_DATA_NOFIL) {
bmc.pv_voltage = get_adc_volts(PVV_C, TRUE, set_range); // read PV voltage on DIFF channels, no filter
} else {
bmc.pv_voltage = lp_filter(get_adc_volts(PVV_C, TRUE, set_range), PVV_C, FALSE); // read PV voltage on DIFF channels
}
}
} else {
return -1;
}
// FIXME
if (HAVE_DIO) {
bmc.datain.D0 = get_dio_bit(8);
bmc.datain.D1 = get_dio_bit(9);
bmc.datain.D2 = get_dio_bit(10);
bmc.datain.D3 = get_dio_bit(11);
bmc.datain.D4 = get_dio_bit(12);
bmc.datain.D5 = get_dio_bit(13);
bmc.datain.D6 = get_dio_bit(14);
bmc.datain.D7 = get_dio_bit(15);
put_dio_bit(0, bmc.dataout.D0);
put_dio_bit(1, bmc.dataout.D1);
put_dio_bit(2, bmc.dataout.D2);
put_dio_bit(3, bmc.dataout.D3);
put_dio_bit(4, bmc.dataout.D4);
put_dio_bit(5, bmc.dataout.D5);
put_dio_bit(6, bmc.dataout.D6);
put_dio_bit(7, bmc.dataout.D7);
} else {
return -2;
}
return 0;
}
void call_filter(double *Y, double *X, int N )
{
int k;
double y;
for (k=0;k<N;k++){
y = X[k];
y = y*y ; // squaring [block 2]
y = filter(y); // high pass 1 order [block 3]
y = y*y; // squaring [block 4]
y = lp_filter(y); // low pass 1 order [block 4]
Y[k]=y*SCALING_FACTOR; // scaling and output [block 4]
}
}
void ADC_read(void) // update all voltage/current readings and set load current in 'currentload' variable
{ // ADC is opened and config'd in main
static uint16_t i, z, change = 0; // used for fast and slow sample loops >256
static int32_t a10_x_t, a10_y_t, a10_z_t;
ClrWdt(); // reset the WDT timer
ADC_zero();
SetChanADC(ADC_CH0); // A0 system
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
vbatol_t = Vin * (ADC0_MV - ADC_NULL + adc_cal[0]); // voltage correction factor
vbatol_t = vbatol_t / 100;
ADC_zero();
SetChanADC(ADC_CH1); // A1 motor
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
solar_t = Vin * (ADC1_MV - ADC_NULL + adc_cal[1]);
solar_t = solar_t / 100;
ADC_zero();
SetChanADC(ADC_CH2); // A2 current_x
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_S; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_S;
a10_x = Vin; // raw ADC value
Vin = Vin - (AMP10_ZERO - ADC_NULL + adc_cal[11]); // set zero NULL0 point
a10_x_t = (long) Vin * (long) (ADC2_MV - ADC_NULL + adc_cal[2]);
if (ABSL(a10_x_t) < AMPZ)
a10_x_t = 0; // zero bit noise
a10_x_t = (long) ((float) a10_x_t / (float) AMP10_SEN_H);
a10_x_t = (long) lp_filter((float) a10_x_t, LP_CURRENT_X, FALSE); // use digital filter
if ((a10_x == NULL0) || (a10_x_t < 0.0)) a10_x_t = 0; // if sensor disconnected read zero
ADC_zero();
SetChanADC(ADC_CH11); // A11 current_y, switch from 3 so we can use external VREF
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_S; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_S; //
a10_y = Vin; // raw ADC value
Vin = Vin - (AMP10_ZERO - ADC_NULL + adc_cal[12]);
a10_y_t = (long) Vin * (long) (ADC3_MV - ADC_NULL + adc_cal[3]);
if (ABSL(a10_y_t) < AMPZ)
a10_y_t = 0; // zero bit noise
a10_y_t = (long) ((float) a10_y_t) / ((float) AMP10_SEN_L);
a10_y_t = (long) lp_filter((float) a10_y_t, LP_CURRENT_Y, FALSE); // use digital filter
if ((a10_y == NULL0) || (a10_y_t < 0.0)) a10_y_t = 0; // if sensor disconnected (zero) read zero current
ADC_zero();
SetChanADC(ADC_CH4); // A5 current_z
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_S; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_S;
a10_z = Vin;
Vin = Vin - (AMP10_ZERO - ADC_NULL + adc_cal[13]); // set zero NULL0 point
a10_z_t = (long) Vin * (long) (ADC4_MV - ADC_NULL + adc_cal[4]);
if (ABSL(a10_z_t) < AMPZ)
a10_z_t = 0; // zero bit noise
a10_z_t = (long) ((float) a10_z_t / (float) AMP10_SEN_L);
a10_z_t = (long) lp_filter((float) a10_z_t, LP_CURRENT_Z, FALSE);
if ((a10_z == NULL0) || (a10_z_t < 0.0)) a10_z_t = 0; // if sensor disconnected read zero
ADC_zero();
SetChanADC(ADC_CH5); // F0 pot x
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawp[0] = Vin;
ADC_zero();
SetChanADC(ADC_CH6); // F1 pot y
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawp[1] = Vin;
ADC_zero();
SetChanADC(ADC_CH7); // F2 pot z
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawp[2] = Vin;
ADC_zero();
SetChanADC(ADC_CH8); // F0 pot max
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawa[0] = Vin;
ADC_zero();
SetChanADC(ADC_CH9); // F1 pot max
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawa[1] = Vin;
ADC_zero();
SetChanADC(ADC_CH10); // F2 pot max
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawa[2] = Vin;
R.motorvoltage = solar_t;
R.systemvoltage = vbatol_t;
if (SYSTEM_STABLE) {
s_crit(HL);
R.current_z = a10_z_t;
R.current_y = a10_y_t;
R.current_x = a10_x_t;
R.pos_x = rawp[XAXIS];
R.pos_y = rawp[YAXIS];
R.pos_z = rawp[ZAXIS];
R.max_x = rawa[XAXIS];
R.max_y = rawa[YAXIS];
R.max_z = rawa[ZAXIS];
e_crit();
for (z = 0; z < MAX_POT; z++) {
motordata[z].pot.pos_actual = rawp[z];
if (ABSI(motordata[z].pot.pos_actual - motordata[z].pot.pos_actual_prev) > motordata[z].pot.pos_change) {
motordata[z].pot.pos_change = ABSI(motordata[z].pot.pos_actual - motordata[z].pot.pos_actual_prev);
}
if (motordata[z].active && (motordata[z].pot.pos_change > motordata[z].pot.limit_change)) {
if (mode.change) {
term_time();
sprintf(bootstr2, " Pot %i Change too high %i\r\n", z, motordata[z].pot.pos_change);
puts2USART(bootstr2);
motordata[z].pot.pos_change = motordata[z].pot.limit_change; // after one message stop and set it to the limit.
} else {
motordata[z].pot.pos_actual = rawp[z]; // set to current pot reading
motordata[z].pot.pos_actual_prev = rawp[z];
motordata[z].pot.pos_change = 0; // pot position change mag
}
}
// Check for POT Dead-Spot readings
if ((motordata[z].pot.pos_change >= POT_MAX_CHANGE) && !mode.qei) motordata[z].pot.cal_failed = TRUE;
if (!motordata[z].active) motordata[z].pot.cal_failed = FALSE; // don't fail inactive motors
motordata[z].pot.pos_actual_prev = motordata[z].pot.pos_actual;
if (motordata[z].pot.pos_actual > motordata[z].pot.high) motordata[z].pot.high = motordata[z].pot.pos_actual; // set adc limits of values X,Y,Z
if (motordata[z].pot.pos_actual < motordata[z].pot.low) motordata[z].pot.low = motordata[z].pot.pos_actual;
if (mode.operate == VIISION_MS) { // use the preset o-100 resistance limits for scaling
motordata[z].pot.high = VIISION_MS_RES_HIGH;
motordata[z].pot.low = VIISION_MS_RES_LOW;
}
motordata[z].pot.offset = motordata[z].pot.low;
motordata[z].pot.span = motordata[z].pot.high - motordata[z].pot.low;
if (motordata[z].pot.span < 0) motordata[z].pot.span = 0;
motordata[z].pot.scale_out = SCALED_FLOAT / motordata[z].pot.span;
motordata[z].pot.scale_in = motordata[z].pot.span / SCALED_FLOAT;
motordata[z].pot.scaled_actual = (int) ((float) (motordata[z].pot.pos_actual - motordata[z].pot.offset) * motordata[z].pot.scale_out);
if (motordata[z].pot.scaled_actual > SCALED) motordata[z].pot.scaled_actual = SCALED;
motordata[z].pot.pos_set = (int) (((float) motordata[z].pot.scaled_set * motordata[z].pot.scale_in) + motordata[z].pot.offset);
}
if (mode.emo) {
term_time();
sprintf(bootstr2, " EMO flag tripped, possible short circuit in the assy wiring.\r\n");
puts2USART(bootstr2);
term_time();
sprintf(bootstr2, " EMO DUMP Current %li, %li, %li : Position %li, %li, %li\r\n", emodump.emo[0], emodump.emo[1], emodump.emo[2], emodump.emo[3], emodump.emo[4], emodump.emo[5]);
puts2USART(bootstr2);
voice2_ticks(40);
while (TRUE) {
emo_display();
buzzer_ticks(200);
ClrWdt(); // reset the WDT timer
}
}
}
ADC_zero(); // ground ADC input
ClrWdt(); // reset the WDT timer
}
