void doSlowAdc(void) {
efiAssertVoid(CUSTOM_ERR_6658, getRemainingStack(chThdGetSelfX())> 32, "lwStAdcSlow");
#if EFI_INTERNAL_ADC
/* Starts an asynchronous ADC conversion operation, the conversion
will be executed in parallel to the current PWM cycle and will
terminate before the next PWM cycle.*/
slowAdc.conversionCount++;
chSysLockFromISR()
;
if (ADC_SLOW_DEVICE.state != ADC_READY &&
ADC_SLOW_DEVICE.state != ADC_COMPLETE &&
ADC_SLOW_DEVICE.state != ADC_ERROR) {
// todo: why and when does this happen? firmwareError(OBD_PCM_Processor_Fault, "ADC slow not ready?");
slowAdc.errorsCount++;
chSysUnlockFromISR()
;
return;
}
adcStartConversionI(&ADC_SLOW_DEVICE, &adcgrpcfgSlow, slowAdc.samples, ADC_BUF_DEPTH_SLOW);
chSysUnlockFromISR()
;
#endif /* EFI_INTERNAL_ADC */
}
static void pwmpcb_fast(PWMDriver *pwmp) {
efiAssertVoid(CUSTOM_ERR_6659, getRemainingStack(chThdGetSelfX())> 32, "lwStAdcFast");
#if EFI_INTERNAL_ADC
(void) pwmp;
/*
* Starts an asynchronous ADC conversion operation, the conversion
* will be executed in parallel to the current PWM cycle and will
* terminate before the next PWM cycle.
*/
chSysLockFromISR()
;
if (ADC_FAST_DEVICE.state != ADC_READY &&
ADC_FAST_DEVICE.state != ADC_COMPLETE &&
ADC_FAST_DEVICE.state != ADC_ERROR) {
fastAdc.errorsCount++;
// todo: when? why? firmwareError(OBD_PCM_Processor_Fault, "ADC fast not ready?");
chSysUnlockFromISR()
;
return;
}
adcStartConversionI(&ADC_FAST_DEVICE, &adcgrpcfg_fast, fastAdc.samples, ADC_BUF_DEPTH_FAST);
chSysUnlockFromISR()
;
fastAdc.conversionCount++;
#endif /* EFI_INTERNAL_ADC */
}
void gadc_lld_timerjobI(GadcTimerJob *pj) {
acg.channelselects = pj->physdev;
acg.trigger = ADC_TRIGGER_TIMER;
acg.frequency = nextfreq;
nextfreq = 0; // Next job use the same timer
adcStartConversionI(&ADCD1, &acg, pj->buffer, pj->todo);
}
static void pwmpcb_slow(PWMDriver *pwmp) {
efiAssertVoid(getRemainingStack(chThdSelf())> 32, "lwStAdcSlow");
#if EFI_INTERNAL_ADC
(void) pwmp;
/* Starts an asynchronous ADC conversion operation, the conversion
will be executed in parallel to the current PWM cycle and will
terminate before the next PWM cycle.*/
slowAdc.conversionCount++;
chSysLockFromIsr()
;
if (ADC_SLOW_DEVICE.state != ADC_READY &&
ADC_SLOW_DEVICE.state != ADC_COMPLETE &&
ADC_SLOW_DEVICE.state != ADC_ERROR) {
// todo: why and when does this happen? firmwareError("ADC slow not ready?");
slowAdc.errorsCount++;
chSysUnlockFromIsr()
;
return;
}
adcStartConversionI(&ADC_SLOW_DEVICE, &adcgrpcfgSlow, slowAdc.samples, ADC_BUF_DEPTH_SLOW);
chSysUnlockFromIsr()
;
#endif
}
void gadc_lld_adc_nontimerI(GadcLldNonTimerData *pgntd) {
/**
* We don't need to calculate num_channels because the AT91SAM7 ADC does this for us.
*/
acg.channelselects = pgntd->physdev;
acg.trigger = ADC_TRIGGER_SOFTWARE;
adcStartConversionI(&ADCD1, &acg, pgntd->buffer, 1);
}
/**
* @brief Starts an ADC conversion.
* @details Starts an asynchronous conversion operation.
* @note The buffer is organized as a matrix of M*N elements where M is the
* channels number configured into the conversion group and N is the
* buffer depth. The samples are sequentially written into the buffer
* with no gaps.
*
* @param[in] adcp pointer to the @p ADCDriver object
* @param[in] grpp pointer to a @p ADCConversionGroup object
* @param[out] samples pointer to the samples buffer
* @param[in] depth buffer depth (matrix rows number). The buffer depth
* must be one or an even number.
*
* @api
*/
void adcStartConversion(ADCDriver *adcp,
const ADCConversionGroup *grpp,
adcsample_t *samples,
size_t depth) {
osalSysLock();
adcStartConversionI(adcp, grpp, samples, depth);
osalSysUnlock();
}
/*
* PWM cyclic callback.
* A new ADC conversion is started.
*/
static void pwmpcb(PWMDriver *pwmp) {
(void)pwmp;
/* Starts an asynchronous ADC conversion operation, the conversion
will be executed in parallel to the current PWM cycle and will
terminate before the next PWM cycle.*/
chSysLockFromIsr();
adcStartConversionI(&ADCD1, &adcgrpcfg, samples, ADC_GRP1_BUF_DEPTH);
chSysUnlockFromIsr();
}
void gadc_lld_adc_timerI(GadcLldTimerData *pgtd) {
/**
* We don't need to calculate num_channels because the AT91SAM7 ADC does this for us.
*/
acg.channelselects = pgtd->physdev;
acg.trigger = pgtd->now ? (ADC_TRIGGER_TIMER|ADC_TRIGGER_SOFTWARE) : ADC_TRIGGER_TIMER;
adcStartConversionI(&ADCD1, &acg, pgtd->buffer, pgtd->count);
/* Next time assume the same (still running) timer */
acg.frequency = 0;
}
/**
* @brief Performs an ADC conversion.
* @details Performs a synchronous conversion operation.
* @note The buffer is organized as a matrix of M*N elements where M is the
* channels number configured into the conversion group and N is the
* buffer depth. The samples are sequentially written into the buffer
* with no gaps.
*
* @param[in] adcp pointer to the @p ADCDriver object
* @param[in] grpp pointer to a @p ADCConversionGroup object
* @param[out] samples pointer to the samples buffer
* @param[in] depth buffer depth (matrix rows number). The buffer depth
* must be one or an even number.
* @return The operation result.
* @retval MSG_OK Conversion finished.
* @retval MSG_RESET The conversion has been stopped using
* @p acdStopConversion() or @p acdStopConversionI(),
* the result buffer may contain incorrect data.
* @retval MSG_TIMEOUT The conversion has been stopped because an hardware
* error.
*
* @api
*/
msg_t adcConvert(ADCDriver *adcp,
const ADCConversionGroup *grpp,
adcsample_t *samples,
size_t depth) {
msg_t msg;
osalSysLock();
osalDbgAssert(adcp->thread == NULL, "already waiting");
adcStartConversionI(adcp, grpp, samples, depth);
msg = osalThreadSuspendS(&adcp->thread);
osalSysUnlock();
return msg;
}
static void pwmpcb_fast(PWMDriver *pwmp) {
#ifdef EFI_INTERNAL_ADC
(void) pwmp;
/* Starts an asynchronous ADC conversion operation, the conversion
will be executed in parallel to the current PWM cycle and will
terminate before the next PWM cycle.*/chSysLockFromIsr()
;
adcStartConversionI(&ADC_FAST, &adcgrpcfg_fast, samples_fast, ADC_GRP1_BUF_DEPTH_FAST);
chSysUnlockFromIsr()
;
#endif
}
/**
* @brief Performs an ADC conversion.
* @details Performs a synchronous conversion operation.
* @note The buffer is organized as a matrix of M*N elements where M is the
* channels number configured into the conversion group and N is the
* buffer depth. The samples are sequentially written into the buffer
* with no gaps.
*
* @param[in] adcp pointer to the @p ADCDriver object
* @param[in] grpp pointer to a @p ADCConversionGroup object
* @param[out] samples pointer to the samples buffer
* @param[in] depth buffer depth (matrix rows number). The buffer depth
* must be one or an even number.
* @return The operation result.
* @retval RDY_OK Conversion finished.
* @retval RDY_RESET The conversion has been stopped using
* @p acdStopConversion() or @p acdStopConversionI(),
* the result buffer may contain incorrect data.
* @retval RDY_TIMEOUT The conversion has been stopped because an hardware
* error.
*
* @api
*/
msg_t adcConvert(ADCDriver *adcp,
const ADCConversionGroup *grpp,
adcsample_t *samples,
size_t depth) {
msg_t msg;
chSysLock();
chDbgAssert(adcp->thread == NULL, "adcConvert(), #1", "already waiting");
adcStartConversionI(adcp, grpp, samples, depth);
adcp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
return msg;
}
void gadc_lld_nontimerjobI(GadcNonTimerJob *pj) {
acg.channelselects = pj->physdev;
acg.trigger = ADC_TRIGGER_SOFTWARE;
adcStartConversionI(&ADCD1, &acg, pj->buffer, 1);
}
/*
* 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 sonarStart(){
sonar_en = TRUE;
adcStartConversionI(&ADCD1, &adcgrpcfg2, sampels, ADC_BUFFER_DEPTH);
}
/*
* GMD control thread, times are in microseconds.
*/
static void motordrive(GPTDriver *gptp) {
(void) gptp;
U32DelayCount = 0;
chSysLockFromIsr();
// Read Input ADC's
adcStartConversionI(&ADCD3, &adcgrp2cfg, InputSamples, ADC_GRP2_BUF_DEPTH);
// Read Feedback ADC's
adcStartConversionI(&ADCD1, &adcgrp1cfg5, vertFeedbackSample, ADC_GRP1_BUF_DEPTH);
adcStartConversionI(&ADCD2, &adcgrp1cfg4, latFeedbackSample, ADC_GRP1_BUF_DEPTH);
chSysUnlockFromIsr();
//TODO undo the constant enable
// Read input switch positions
// U8EnableDriveSwitch = 1;
U8EnableDriveSwitch = palReadPad(GPIOD, GPIOD_PIN8); // PD8
// U8PosnVelModeSwitch = 1; // PD9
U8PosnVelModeSwitch = palReadPad(GPIOD, GPIOD_PIN9); // PD9
// Handle ENABLE on GMD and LEDs on GFE
if(U8EnableDriveSwitch > 0 && U8ShellEnable > 0) {
// Turn ON enable
palSetPad(GPIOD, GPIOD_PIN10);
palSetPad(GPIOD, GPIOD_PIN6);
// // Turn Green LED on, Red LED off
// palSetPad(GPIOD, GPIOD_PIN0);
// palClearPad(GPIOD, GPIOD_PIN1);
// Interlock drives on mode change
if(U8ShellMode == 2) {
if(U8PosnVelModeSwitch != U8PrevPosnVelModeSwitchState) {
vertAxisStruct.U8DriveIsInterlocked = 1;
latAxisStruct.U8DriveIsInterlocked = 1;
}
}
else {
if(U8ShellMode != U8PrevPosnVelModeSwitchState) {
vertAxisStruct.U8DriveIsInterlocked = 1;
latAxisStruct.U8DriveIsInterlocked = 1;
}
}
}
else {
// Otherwise enable is OFF
palClearPad(GPIOD, GPIOD_PIN10);
palClearPad(GPIOD, GPIOD_PIN6);
// // Turn Green LED off, Red LED on
// palClearPad(GPIOD, GPIOG_PIN0);
// palSetPad(GPIOD, GPIOD_PIN1);
// Lock out drives
vertAxisStruct.U8DriveIsInterlocked = 1;
latAxisStruct.U8DriveIsInterlocked = 1;
}
// Interlock drives on mode change
if(U8ShellMode == 2) {
U8PrevPosnVelModeSwitchState = U8PosnVelModeSwitch;
}
else {
U8PrevPosnVelModeSwitchState = U8ShellMode;
}
// U8PrevPosnVelModeSwitchState = U8PosnVelModeSwitch;
processLeds();
// Handle HW Watchdog on GMD
if(u8WatchDogCycleCount > 7){
u8WatchDogCycleCount = 0;
palTogglePad(GPIOD, GPIOD_PIN11);
palTogglePad(GPIOD, GPIOD_PIN5);
}
else
u8WatchDogCycleCount++;
return;
}
