void adc_callback(ADCDriver * adcp, adcsample_t *buffer, size_t n) {
(void)adcp;
(void)buffer;
(void)n;
cb_arg++;
if (adcp->grpp->circular && cb_arg == cb_expect) {
osalSysLockFromISR();
adcStopConversionI(adcp);
osalSysUnlockFromISR();
}
}
/*
* Analog I/O thread
*/
msg_t thdAnalogIO(void *arg)
{
(void)arg;
uint32_t timeStamp=0;
float i2cAdcPos[I2C_MAX_CHANNELS]; // 5 servos + resistive foam in the clamp
float i2cAdcCurrent[I2C_MAX_CHANNELS]; // 5 servo consumption
bool_t i2cOk = TRUE;
uint32_t lowPassIteration=0;
/*
* initialize the Analog IO (5 pins)
*/
initAnalogIO ();
chRegSetThreadName("thdAnalogIO");
/* init i2c part */
if (initI2cDriver (&i2c2) != TRUE) {
DebugTrace ("I2C (i2c2) init FAILED");
syslog (LOG_FATAL, "I2C (i2c2) FAIL");
i2cOk = FALSE;
}
while (!chThdShouldTerminate()) {
chSysLock ();
adcStopConversionI (&ADCD3);
adcStartConversionI (&ADCD3, &adcgrpcfg3, samples3, ADC_BUF_DEPTH);
chSysUnlock ();
if (i2cOk && ( isActive(Analog_inCurrent) || isActive(Analog_inServiPos))) {
static float medianFilterBuffer[ADC_MEDIAN_SIZE] [SERVO_COUNT+1] [MEDIAN_SIZE] = {{{0.0f}}};
// POSITION
if (i2cGetADC_ADS7828_Val (i2c2.driver, I2C_ADC_SERVOPOS_ADR_OFFSET, 0b01011111, i2cAdcPos)) {
DebugTrace ("i2cGetADC_ADS7828_Val error for position sensor");
syslog (LOG_ERROR, "ADS7828 POS FAIL");
} else {
// first of all, the median filter
static bool firstTime=TRUE;
if (firstTime) {
firstTime = FALSE;
for (uint32_t i=0; i<SERVO_COUNT+1; i++) {
i2cAvPos[i] = i2cAdcPos[i];
}
} else {
for (uint32_t i=0; i<SERVO_COUNT+1; i++) {
// shift array and discard oldest value
memmove (&medianFilterBuffer[ADC_SERVO_POS][i][0],
&medianFilterBuffer[ADC_SERVO_POS][i][1],
sizeof(float) * MEDIAN_SIZE-1);
// copy new incomming value
medianFilterBuffer[ADC_SERVO_POS][i][MEDIAN_SIZE-1] = i2cAdcPos[i];
// copy to sort buffer
memcpy (&medianFilterBuffer[ADC_VALUE_SORT][i][0],
&medianFilterBuffer[ADC_SERVO_POS][i][0],
sizeof(float) * MEDIAN_SIZE);
// sort the array
qsort (&medianFilterBuffer[ADC_VALUE_SORT][i][0], MEDIAN_SIZE, sizeof(float),
&qsortCompareFloatCb);
// eliminate lowest and highest values
const float medianMean = (medianFilterBuffer[ADC_VALUE_SORT][i][1] +
medianFilterBuffer[ADC_VALUE_SORT][i][2] + medianFilterBuffer[ADC_VALUE_SORT][i][3])/3.f;
// then low pass filter
lowPassIteration++;
i2cAvPos[i] = (( i2cAvPos[i]*(float)LOW_PASS_N) + medianMean)/ (float) (LOW_PASS_N+1);
}
}
}
// CURRENT
if (i2cGetADC_ADS7828_Val (i2c2.driver, I2C_ADC_CURRENT_ADR_OFFSET, 0b00111111, i2cAdcCurrent)) {
DebugTrace ("i2cGetADC_ADS7828_Val error for current sensor");
syslog (LOG_ERROR, "ADS7828 CURRENT FAIL");
} else {
//for (uint32_t j=0; j<SERVO_COUNT+1; j++) {
//DebugTrace ("RC[%d]=%d", j, (int) (i2cAdcCurrent[j]*4096.0f));
//}
static bool firstTime=TRUE;
if (firstTime) {
firstTime = FALSE;
for (uint32_t i=0; i<SERVO_COUNT+1; i++) {
i2cAvCurrent[i] = i2cAdcCurrent[i];
}
} else {
for (uint32_t i=0; i<SERVO_COUNT+1; i++) {
i2cAvCurrent[i] = ((i2cAvCurrent[i]*(float)LOW_PASS_N) +
i2cAdcCurrent[i])/ (float) (LOW_PASS_N+1);
}
}
}
}
// check on power supply voltage
if (lowPassIteration >= 100) { // wait for filter to stabilize
if (analogGet5VoltPowerVoltage() < MIN_5VOLTS_POWER_SUPPLY_VOLTAGE) {
if (isServoEngaged (SERVO_ALL_SERVOS)) {
// have to test validity of analogGet5VoltPowerVoltage() before using it
// servoDisengage (SERVO_ALL_SERVOS);
#pragma message \
"enable servoDisengage (SERVO_ALL_SERVOS) in " __FILE__ \
" after testing validity of analogGet5VoltPowerVoltage()"
syslog (LOG_ERROR, "Surcharge Alim");
syslog (LOG_ERROR, "Servos ** OFF **");
syslog (LOG_INFO, "rearmer : Bouton VERT");
}
}
}
// check on current intensity
for (uint32_t i=0; i<SERVO_COUNT; i++) {
if (i2cAvCurrent[i] > max_current_intensity_use[i]) {
if (isServoEngaged (i)) {
servoDisengage (i);
syslog (LOG_ERROR, "Surcharge Servo %d", i);
syslog (LOG_ERROR, "Servos %d** OFF **", i);
syslog (LOG_INFO, "rearmer : Bouton VERT");
}
}
}
if (isActive(Analog_inCommand)) {
if ((chTimeNow() - timeStamp) >= TIME_STEP) {
timeStamp = chTimeNow();
for (uint32_t i=0; i<SERVO_COUNT ; i++) {
const float adcV = analogGetCmd(i);
servoSetPos (i, adcV);
/* if (chTimeNow() > 5000) */
/* chprintf (chp, "Apos=%.4f\r\n", adcV); */
}
}
} else {
chThdSleepMilliseconds(1);
}
}
closeAnalogIO();
i2cStop (i2c2.driver);
return 0;
}
void sonarStop(){
sonar_en = FALSE;
adcStopConversionI(&ADCD1);
}