/* 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 }