diff --git a/hal/include/HAL/DIO.h b/hal/include/HAL/DIO.h index a904c0dcb7..0ec2c14cd5 100644 --- a/hal/include/HAL/DIO.h +++ b/hal/include/HAL/DIO.h @@ -10,6 +10,8 @@ #include extern "C" { +// the following 2 functions are here as they will be changed with +// the handle changes to be DIO exclusive. void* initializeDigitalPort(void* port_pointer, int32_t* status); void freeDigitalPort(void* digital_port_pointer); diff --git a/hal/include/HAL/HAL.h b/hal/include/HAL/HAL.h index 7c20b9de74..a982cecf74 100644 --- a/hal/include/HAL/HAL.h +++ b/hal/include/HAL/HAL.h @@ -28,7 +28,6 @@ #include "Power.h" #include "Relay.h" #include "SPI.h" -#include "SPIAccumulator.h" #include "Semaphore.h" #include "SerialPort.h" #include "Solenoid.h" diff --git a/hal/include/HAL/SPIAccumulator.h b/hal/include/HAL/SPIAccumulator.h deleted file mode 100644 index 1e31c403b8..0000000000 --- a/hal/include/HAL/SPIAccumulator.h +++ /dev/null @@ -1,28 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#pragma once - -#include - -extern "C" { -void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, - uint8_t xfer_size, uint32_t valid_mask, - uint32_t valid_value, uint8_t data_shift, - uint8_t data_size, bool is_signed, bool big_endian, - int32_t* status); -void spiFreeAccumulator(uint8_t port, int32_t* status); -void spiResetAccumulator(uint8_t port, int32_t* status); -void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t* status); -void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t* status); -int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t* status); -int64_t spiGetAccumulatorValue(uint8_t port, int32_t* status); -uint32_t spiGetAccumulatorCount(uint8_t port, int32_t* status); -double spiGetAccumulatorAverage(uint8_t port, int32_t* status); -void spiGetAccumulatorOutput(uint8_t port, int64_t* value, uint32_t* count, - int32_t* status); -} diff --git a/hal/lib/athena/Counter.cpp b/hal/lib/athena/Counter.cpp new file mode 100644 index 0000000000..7156cfc677 --- /dev/null +++ b/hal/lib/athena/Counter.cpp @@ -0,0 +1,340 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/Counter.h" + +#include "DigitalInternal.h" +#include "HAL/HAL.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +extern "C" { +struct counter_t { + tCounter* counter; + uint32_t index; +}; +typedef struct counter_t Counter; + +static hal::Resource* counters = nullptr; + +void* initializeCounter(Mode mode, uint32_t* index, int32_t* status) { + hal::Resource::CreateResourceObject(&counters, tCounter::kNumSystems); + *index = counters->Allocate("Counter"); + if (*index == ~0ul) { + *status = NO_AVAILABLE_RESOURCES; + return nullptr; + } + Counter* counter = new Counter(); + counter->counter = tCounter::create(*index, status); + counter->counter->writeConfig_Mode(mode, status); + counter->counter->writeTimerConfig_AverageSize(1, status); + counter->index = *index; + return counter; +} + +void freeCounter(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + if (!counter) return; + delete counter->counter; + counters->Free(counter->index); +} + +void setCounterAverageSize(void* counter_pointer, int32_t size, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeTimerConfig_AverageSize(size, status); +} + +/** + * Set the source object that causes the counter to count up. + * Set the up counting DigitalSource. + */ +void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + + uint8_t module; + + remapDigitalSource(analogTrigger, pin, module); + + counter->counter->writeConfig_UpSource_Module(module, status); + counter->counter->writeConfig_UpSource_Channel(pin, status); + counter->counter->writeConfig_UpSource_AnalogTrigger(analogTrigger, status); + + if (counter->counter->readConfig_Mode(status) == kTwoPulse || + counter->counter->readConfig_Mode(status) == kExternalDirection) { + setCounterUpSourceEdge(counter_pointer, true, false, status); + } + counter->counter->strobeReset(status); +} + +/** + * Set the edge sensitivity on an up counting source. + * Set the up source to either detect rising edges or falling edges. + */ +void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, + bool fallingEdge, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_UpRisingEdge(risingEdge, status); + counter->counter->writeConfig_UpFallingEdge(fallingEdge, status); +} + +/** + * Disable the up counting source to the counter. + */ +void clearCounterUpSource(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_UpFallingEdge(false, status); + counter->counter->writeConfig_UpRisingEdge(false, status); + // Index 0 of digital is always 0. + counter->counter->writeConfig_UpSource_Channel(0, status); + counter->counter->writeConfig_UpSource_AnalogTrigger(false, status); +} + +/** + * Set the source object that causes the counter to count down. + * Set the down counting DigitalSource. + */ +void setCounterDownSource(void* counter_pointer, uint32_t pin, + bool analogTrigger, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + unsigned char mode = counter->counter->readConfig_Mode(status); + if (mode != kTwoPulse && mode != kExternalDirection) { + // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only + // supports DownSource in TwoPulse and ExternalDirection modes."); + *status = PARAMETER_OUT_OF_RANGE; + return; + } + + uint8_t module; + + remapDigitalSource(analogTrigger, pin, module); + + counter->counter->writeConfig_DownSource_Module(module, status); + counter->counter->writeConfig_DownSource_Channel(pin, status); + counter->counter->writeConfig_DownSource_AnalogTrigger(analogTrigger, status); + + setCounterDownSourceEdge(counter_pointer, true, false, status); + counter->counter->strobeReset(status); +} + +/** + * Set the edge sensitivity on a down counting source. + * Set the down source to either detect rising edges or falling edges. + */ +void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, + bool fallingEdge, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_DownRisingEdge(risingEdge, status); + counter->counter->writeConfig_DownFallingEdge(fallingEdge, status); +} + +/** + * Disable the down counting source to the counter. + */ +void clearCounterDownSource(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_DownFallingEdge(false, status); + counter->counter->writeConfig_DownRisingEdge(false, status); + // Index 0 of digital is always 0. + counter->counter->writeConfig_DownSource_Channel(0, status); + counter->counter->writeConfig_DownSource_AnalogTrigger(false, status); +} + +/** + * Set standard up / down counting mode on this counter. + * Up and down counts are sourced independently from two inputs. + */ +void setCounterUpDownMode(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_Mode(kTwoPulse, status); +} + +/** + * Set external direction mode on this counter. + * Counts are sourced on the Up counter input. + * The Down counter input represents the direction to count. + */ +void setCounterExternalDirectionMode(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_Mode(kExternalDirection, status); +} + +/** + * Set Semi-period mode on this counter. + * Counts up on both rising and falling edges. + */ +void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_Mode(kSemiperiod, status); + counter->counter->writeConfig_UpRisingEdge(highSemiPeriod, status); + setCounterUpdateWhenEmpty(counter_pointer, false, status); +} + +/** + * Configure the counter to count in up or down based on the length of the input + * pulse. + * This mode is most useful for direction sensitive gear tooth sensors. + * @param threshold The pulse length beyond which the counter counts the + * opposite direction. Units are seconds. + */ +void setCounterPulseLengthMode(void* counter_pointer, double threshold, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeConfig_Mode(kPulseLength, status); + counter->counter->writeConfig_PulseLengthThreshold( + (uint32_t)(threshold * 1.0e6) * kSystemClockTicksPerMicrosecond, status); +} + +/** + * Get the Samples to Average which specifies the number of samples of the timer + * to + * average when calculating the period. Perform averaging to account for + * mechanical imperfections or as oversampling to increase resolution. + * @return SamplesToAverage The number of samples being averaged (from 1 to 127) + */ +int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + return counter->counter->readTimerConfig_AverageSize(status); +} + +/** + * Set the Samples to Average which specifies the number of samples of the timer + * to average when calculating the period. Perform averaging to account for + * mechanical imperfections or as oversampling to increase resolution. + * @param samplesToAverage The number of samples to average from 1 to 127. + */ +void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + if (samplesToAverage < 1 || samplesToAverage > 127) { + *status = PARAMETER_OUT_OF_RANGE; + } + counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status); +} + +/** + * Reset the Counter to zero. + * Set the counter value to zero. This doesn't effect the running state of the + * counter, just sets the current value to zero. + */ +void resetCounter(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->strobeReset(status); +} + +/** + * Read the current counter value. + * Read the value at this instant. It may still be running, so it reflects the + * current value. Next time it is read, it might have a different value. + */ +int32_t getCounter(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + int32_t value = counter->counter->readOutput_Value(status); + return value; +} + +/* + * Get the Period of the most recent count. + * Returns the time interval of the most recent count. This can be used for + * velocity calculations to determine shaft speed. + * @returns The period of the last two pulses in units of seconds. + */ +double getCounterPeriod(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + tCounter::tTimerOutput output = counter->counter->readTimerOutput(status); + double period; + if (output.Stalled) { + // Return infinity + double zero = 0.0; + period = 1.0 / zero; + } else { + // output.Period is a fixed point number that counts by 2 (24 bits, 25 + // integer bits) + period = (double)(output.Period << 1) / (double)output.Count; + } + return period * 2.5e-8; // result * timebase (currently 40ns) +} + +/** + * Set the maximum period where the device is still considered "moving". + * Sets the maximum period where the device is considered moving. This value is + * used to determine the "stopped" state of the counter using the GetStopped + * method. + * @param maxPeriod The maximum period where the counted device is considered + * moving in seconds. + */ +void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 4.0e8), + status); +} + +/** + * Select whether you want to continue updating the event timer output when + * there are no samples captured. The output of the event timer has a buffer of + * periods that are averaged and posted to a register on the FPGA. When the + * timer detects that the event source has stopped (based on the MaxPeriod) the + * buffer of samples to be averaged is emptied. If you enable the update when + * empty, you will be notified of the stopped source and the event time will + * report 0 samples. If you disable update when empty, the most recent average + * will remain on the output until a new sample is acquired. You will never see + * 0 samples output (except when there have been no events since an FPGA reset) + * and you will likely not see the stopped bit become true (since it is updated + * at the end of an average and there are no samples to average). + */ +void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeTimerConfig_UpdateWhenEmpty(enabled, status); +} + +/** + * Determine if the clock is stopped. + * Determine if the clocked input is stopped based on the MaxPeriod value set + * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the + * device (and counter) are assumed to be stopped and it returns true. + * @return Returns true if the most recent counter period exceeds the MaxPeriod + * value set by SetMaxPeriod. + */ +bool getCounterStopped(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + return counter->counter->readTimerOutput_Stalled(status); +} + +/** + * The last direction the counter value changed. + * @return The last direction the counter value changed. + */ +bool getCounterDirection(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + bool value = counter->counter->readOutput_Direction(status); + return value; +} + +/** + * Set the Counter to return reversed sensing on the direction. + * This allows counters to change the direction they are counting in the case of + * 1X and 2X quadrature encoding only. Any other counter mode isn't supported. + * @param reverseDirection true if the value counted should be negated. + */ +void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + if (counter->counter->readConfig_Mode(status) == kExternalDirection) { + if (reverseDirection) + setCounterDownSourceEdge(counter_pointer, true, true, status); + else + setCounterDownSourceEdge(counter_pointer, false, true, status); + } +} +} diff --git a/hal/lib/athena/DIO.cpp b/hal/lib/athena/DIO.cpp new file mode 100644 index 0000000000..3c8788a177 --- /dev/null +++ b/hal/lib/athena/DIO.cpp @@ -0,0 +1,423 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/DIO.h" + +#include + +#include "DigitalInternal.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +// Create a mutex to protect changes to the digital output values +static priority_recursive_mutex digitalDIOMutex; + +extern "C" { + +/** + * Create a new instance of a digital port. + */ +void* initializeDigitalPort(void* port_pointer, int32_t* status) { + initializeDigital(status); + Port* port = (Port*)port_pointer; + + // Initialize port structure + DigitalPort* digital_port = new DigitalPort(); + digital_port->port = *port; + + return digital_port; +} + +void freeDigitalPort(void* digital_port_pointer) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + delete port; +} + +/** + * Allocate a DO PWM Generator. + * Allocate PWM generators so that they are not accidentally reused. + * + * @return PWM Generator refnum + */ +void* allocatePWM(int32_t* status) { + return (void*)DO_PWMGenerators->Allocate("DO_PWM"); +} + +/** + * Free the resource associated with a DO PWM generator. + * + * @param pwmGenerator The pwmGen to free that was allocated with + * AllocateDO_PWM() + */ +void freePWM(void* pwmGenerator, int32_t* status) { + uint32_t id = (uint32_t)pwmGenerator; + if (id == ~0ul) return; + DO_PWMGenerators->Free(id); +} + +/** + * Change the frequency of the DO PWM generator. + * + * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is + * logarithmic. + * + * @param rate The frequency to output all digital output PWM signals. + */ +void setPWMRate(double rate, int32_t* status) { + // Currently rounding in the log rate domain... heavy weight toward picking a + // higher freq. + // TODO: Round in the linear rate domain. + uint8_t pwmPeriodPower = (uint8_t)( + log(1.0 / (pwmSystem->readLoopTiming(status) * 0.25E-6 * rate)) / + log(2.0) + + 0.5); + digitalSystem->writePWMPeriodPower(pwmPeriodPower, status); +} + +/** + * Configure the duty-cycle of the PWM generator + * + * @param pwmGenerator The generator index reserved by AllocateDO_PWM() + * @param dutyCycle The percent duty cycle to output [0..1]. + */ +void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t* status) { + uint32_t id = (uint32_t)pwmGenerator; + if (id == ~0ul) return; + if (dutyCycle > 1.0) dutyCycle = 1.0; + if (dutyCycle < 0.0) dutyCycle = 0.0; + float rawDutyCycle = 256.0 * dutyCycle; + if (rawDutyCycle > 255.5) rawDutyCycle = 255.5; + { + std::lock_guard sync(digitalPwmMutex); + uint8_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status); + if (pwmPeriodPower < 4) { + // The resolution of the duty cycle drops close to the highest + // frequencies. + rawDutyCycle = rawDutyCycle / pow(2.0, 4 - pwmPeriodPower); + } + if (id < 4) + digitalSystem->writePWMDutyCycleA(id, (uint8_t)rawDutyCycle, status); + else + digitalSystem->writePWMDutyCycleB(id - 4, (uint8_t)rawDutyCycle, status); + } +} + +/** + * Configure which DO channel the PWM signal is output on + * + * @param pwmGenerator The generator index reserved by AllocateDO_PWM() + * @param channel The Digital Output channel to output on + */ +void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t* status) { + uint32_t id = (uint32_t)pwmGenerator; + if (id > 5) return; + digitalSystem->writePWMOutputSelect(id, pin, status); +} + +/** + * Allocate Digital I/O channels. + * Allocate channels so that they are not accidently reused. Also the direction + * is set at the time of the allocation. + * + * @param channel The Digital I/O channel + * @param input If true open as input; if false open as output + * @return Was successfully allocated + */ +bool allocateDIO(void* digital_port_pointer, bool input, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + char buf[64]; + snprintf(buf, 64, "DIO %d", port->port.pin); + if (DIOChannels->Allocate(port->port.pin, buf) == ~0ul) { + *status = RESOURCE_IS_ALLOCATED; + return false; + } + + { + std::lock_guard sync(digitalDIOMutex); + + tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status); + + if (port->port.pin < kNumHeaders) { + uint32_t bitToSet = 1 << port->port.pin; + if (input) { + outputEnable.Headers = + outputEnable.Headers & (~bitToSet); // clear the bit for read + } else { + outputEnable.Headers = + outputEnable.Headers | bitToSet; // set the bit for write + } + } else { + uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); + + // Disable special functions on this pin + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, + status); + + if (input) { + outputEnable.MXP = + outputEnable.MXP & (~bitToSet); // clear the bit for read + } else { + outputEnable.MXP = + outputEnable.MXP | bitToSet; // set the bit for write + } + } + + digitalSystem->writeOutputEnable(outputEnable, status); + } + return true; +} + +/** + * Free the resource associated with a digital I/O channel. + * + * @param channel The Digital I/O channel to free + */ +void freeDIO(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!port) return; + DIOChannels->Free(port->port.pin); +} + +/** + * Write a digital I/O bit to the FPGA. + * Set a single value on a digital I/O channel. + * + * @param channel The Digital I/O channel + * @param value The state to set the digital channel (if it is configured as an + * output) + */ +void setDIO(void* digital_port_pointer, short value, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (value != 0 && value != 1) { + if (value != 0) value = 1; + } + { + std::lock_guard sync(digitalDIOMutex); + tDIO::tDO currentDIO = digitalSystem->readDO(status); + + if (port->port.pin < kNumHeaders) { + if (value == 0) { + currentDIO.Headers = currentDIO.Headers & ~(1 << port->port.pin); + } else if (value == 1) { + currentDIO.Headers = currentDIO.Headers | (1 << port->port.pin); + } + } else { + if (value == 0) { + currentDIO.MXP = + currentDIO.MXP & ~(1 << remapMXPChannel(port->port.pin)); + } else if (value == 1) { + currentDIO.MXP = + currentDIO.MXP | (1 << remapMXPChannel(port->port.pin)); + } + + uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, + status); + } + digitalSystem->writeDO(currentDIO, status); + } +} + +/** + * Read a digital I/O bit from the FPGA. + * Get a single value from a digital I/O channel. + * + * @param channel The digital I/O channel + * @return The state of the specified channel + */ +bool getDIO(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + tDIO::tDI currentDIO = digitalSystem->readDI(status); + // Shift 00000001 over channel-1 places. + // AND it against the currentDIO + // if it == 0, then return false + // else return true + + if (port->port.pin < kNumHeaders) { + return ((currentDIO.Headers >> port->port.pin) & 1) != 0; + } else { + // Disable special functions + uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, + status); + + return ((currentDIO.MXP >> remapMXPChannel(port->port.pin)) & 1) != 0; + } +} + +/** + * Read the direction of a the Digital I/O lines + * A 1 bit means output and a 0 bit means input. + * + * @param channel The digital I/O channel + * @return The direction of the specified channel + */ +bool getDIODirection(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + tDIO::tOutputEnable currentOutputEnable = + digitalSystem->readOutputEnable(status); + // Shift 00000001 over port->port.pin-1 places. + // AND it against the currentOutputEnable + // if it == 0, then return false + // else return true + + if (port->port.pin < kNumHeaders) { + return ((currentOutputEnable.Headers >> port->port.pin) & 1) != 0; + } else { + return ((currentOutputEnable.MXP >> remapMXPChannel(port->port.pin)) & 1) != + 0; + } +} + +/** + * Generate a single pulse. + * Write a pulse to the specified digital output channel. There can only be a + * single pulse going at any time. + * + * @param channel The Digital Output channel that the pulse should be output on + * @param pulseLength The active length of the pulse (in seconds) + */ +void pulse(void* digital_port_pointer, double pulseLength, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + tDIO::tPulse pulse; + + if (port->port.pin < kNumHeaders) { + pulse.Headers = 1 << port->port.pin; + } else { + pulse.MXP = 1 << remapMXPChannel(port->port.pin); + } + + digitalSystem->writePulseLength( + (uint8_t)(1.0e9 * pulseLength / (pwmSystem->readLoopTiming(status) * 25)), + status); + digitalSystem->writePulse(pulse, status); +} + +/** + * Check a DIO line to see if it is currently generating a pulse. + * + * @return A pulse is in progress + */ +bool isPulsing(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); + + if (port->port.pin < kNumHeaders) { + return (pulseRegister.Headers & (1 << port->port.pin)) != 0; + } else { + return (pulseRegister.MXP & (1 << remapMXPChannel(port->port.pin))) != 0; + } +} + +/** + * Check if any DIO line is currently generating a pulse. + * + * @return A pulse on some line is in progress + */ +bool isAnyPulsing(int32_t* status) { + tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); + return pulseRegister.Headers != 0 && pulseRegister.MXP != 0; +} + +/** + * Write the filter index from the FPGA. + * Set the filter index used to filter out short pulses. + * + * @param digital_port_pointer The digital I/O channel + * @param filter_index The filter index. Must be in the range 0 - 3, + * where 0 means "none" and 1 - 3 means filter # filter_index - 1. + */ +void setFilterSelect(void* digital_port_pointer, int filter_index, + int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + + std::lock_guard sync(digitalDIOMutex); + if (port->port.pin < kNumHeaders) { + digitalSystem->writeFilterSelectHdr(port->port.pin, filter_index, status); + } else { + digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->port.pin), + filter_index, status); + } +} + +/** + * Read the filter index from the FPGA. + * Get the filter index used to filter out short pulses. + * + * @param digital_port_pointer The digital I/O channel + * @return filter_index The filter index. Must be in the range 0 - 3, + * where 0 means "none" and 1 - 3 means filter # filter_index - 1. + */ +int getFilterSelect(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + + std::lock_guard sync(digitalDIOMutex); + if (port->port.pin < kNumHeaders) { + return digitalSystem->readFilterSelectHdr(port->port.pin, status); + } else { + return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->port.pin), + status); + } +} + +/** + * Set the filter period for the specified filter index. + * + * Set the filter period in FPGA cycles. Even though there are 2 different + * filter index domains (MXP vs HDR), ignore that distinction for now since it + * compilicates the interface. That can be changed later. + * + * @param filter_index The filter index, 0 - 2. + * @param value The number of cycles that the signal must not transition to be + * counted as a transition. + */ +void setFilterPeriod(int filter_index, uint32_t value, int32_t* status) { + std::lock_guard sync(digitalDIOMutex); + digitalSystem->writeFilterPeriodHdr(filter_index, value, status); + if (*status == 0) { + digitalSystem->writeFilterPeriodMXP(filter_index, value, status); + } +} + +/** + * Get the filter period for the specified filter index. + * + * Get the filter period in FPGA cycles. Even though there are 2 different + * filter index domains (MXP vs HDR), ignore that distinction for now since it + * compilicates the interface. Set status to NiFpga_Status_SoftwareFault if the + * filter values miss-match. + * + * @param filter_index The filter index, 0 - 2. + * @param value The number of cycles that the signal must not transition to be + * counted as a transition. + */ +uint32_t getFilterPeriod(int filter_index, int32_t* status) { + uint32_t hdr_period = 0; + uint32_t mxp_period = 0; + { + std::lock_guard sync(digitalDIOMutex); + hdr_period = digitalSystem->readFilterPeriodHdr(filter_index, status); + if (*status == 0) { + mxp_period = digitalSystem->readFilterPeriodMXP(filter_index, status); + } + } + if (hdr_period != mxp_period) { + *status = NiFpga_Status_SoftwareFault; + return -1; + } + return hdr_period; +} +} diff --git a/hal/lib/athena/Digital.cpp b/hal/lib/athena/Digital.cpp deleted file mode 100644 index 781a0f3726..0000000000 --- a/hal/lib/athena/Digital.cpp +++ /dev/null @@ -1,2019 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include -#include - -#include - -#include "ChipObject.h" -#include "FRC_NetworkCommunication/LoadOut.h" -#include "HAL/Counter.h" -#include "HAL/DIO.h" -#include "HAL/Encoder.h" -#include "HAL/HAL.h" -#include "HAL/I2C.h" -#include "HAL/PWM.h" -#include "HAL/Port.h" -#include "HAL/Relay.h" -#include "HAL/SPI.h" -#include "HAL/SPIAccumulator.h" -#include "HAL/cpp/Resource.h" -#include "HAL/cpp/priority_mutex.h" -#include "i2clib/i2c-lib.h" -#include "spilib/spi-lib.h" - -static_assert(sizeof(uint32_t) <= sizeof(void*), - "This file shoves uint32_ts into pointers."); - -static const uint32_t kExpectedLoopTiming = 40; -static const uint32_t kDigitalPins = 26; -static const uint32_t kPwmPins = 20; -static const uint32_t kRelayPins = 8; -static const uint32_t kNumHeaders = 10; // Number of non-MXP pins - -/** - * kDefaultPwmPeriod is in ms - * - * - 20ms periods (50 Hz) are the "safest" setting in that this works for all - * devices - * - 20ms periods seem to be desirable for Vex Motors - * - 20ms periods are the specified period for HS-322HD servos, but work - * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums - * and get hot; by 5.0ms the hum is nearly continuous - * - 10ms periods work well for Victor 884 - * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed - * controllers. Due to the shipping firmware on the Jaguar, we can't run the - * update period less than 5.05 ms. - * - * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period - * scaling is implemented as an output squelch to get longer periods for old - * devices. - */ -static const float kDefaultPwmPeriod = 5.05; -/** - * kDefaultPwmCenter is the PWM range center in ms - */ -static const float kDefaultPwmCenter = 1.5; -/** - * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint - */ -static const int32_t kDefaultPwmStepsDown = 1000; -static const int32_t kPwmDisabled = 0; - -struct DigitalPort { - Port port; - uint32_t PWMGeneratorID; -}; - -// Create a mutex to protect changes to the digital output values -static priority_recursive_mutex digitalDIOMutex; -// Create a mutex to protect changes to the relay values -static priority_recursive_mutex digitalRelayMutex; -// Create a mutex to protect changes to the DO PWM config -static priority_recursive_mutex digitalPwmMutex; -static priority_recursive_mutex digitalI2COnBoardMutex; -static priority_recursive_mutex digitalI2CMXPMutex; - -static tDIO* digitalSystem = NULL; -static tRelay* relaySystem = NULL; -static tPWM* pwmSystem = NULL; -static hal::Resource* DIOChannels = NULL; -static hal::Resource* DO_PWMGenerators = NULL; -static hal::Resource* PWMChannels = NULL; - -static bool digitalSystemsInitialized = false; - -static uint8_t i2COnboardObjCount = 0; -static uint8_t i2CMXPObjCount = 0; -static uint8_t i2COnBoardHandle = 0; -static uint8_t i2CMXPHandle = 0; - -static int32_t m_spiCS0Handle = 0; -static int32_t m_spiCS1Handle = 0; -static int32_t m_spiCS2Handle = 0; -static int32_t m_spiCS3Handle = 0; -static int32_t m_spiMXPHandle = 0; -static priority_recursive_mutex spiOnboardSemaphore; -static priority_recursive_mutex spiMXPSemaphore; -static tSPI* spiSystem; - -extern "C" { - -struct SPIAccumulator { - void* notifier = nullptr; - uint64_t triggerTime; - uint32_t period; - - int64_t value = 0; - uint32_t count = 0; - int32_t last_value = 0; - - int32_t center = 0; - int32_t deadband = 0; - - uint8_t cmd[4]; // command to send (up to 4 bytes) - uint32_t valid_mask; - uint32_t valid_value; - int32_t data_max; // one more than max data value - int32_t data_msb_mask; // data field MSB mask (for signed) - uint8_t data_shift; // data field shift right amount, in bits - uint8_t xfer_size; // SPI transfer size, in bytes (up to 4) - uint8_t port; - bool is_signed; // is data field signed? - bool big_endian; // is response big endian? -}; -SPIAccumulator* spiAccumulators[5] = {nullptr, nullptr, nullptr, nullptr, - nullptr}; - -/** - * Initialize the digital system. - */ -void initializeDigital(int32_t* status) { - if (digitalSystemsInitialized) return; - - hal::Resource::CreateResourceObject(&DIOChannels, - tDIO::kNumSystems * kDigitalPins); - hal::Resource::CreateResourceObject( - &DO_PWMGenerators, - tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements); - hal::Resource::CreateResourceObject(&PWMChannels, - tPWM::kNumSystems * kPwmPins); - digitalSystem = tDIO::create(status); - - // Relay Setup - relaySystem = tRelay::create(status); - - // Turn off all relay outputs. - relaySystem->writeValue_Forward(0, status); - relaySystem->writeValue_Reverse(0, status); - - // PWM Setup - pwmSystem = tPWM::create(status); - - // Make sure that the 9403 IONode has had a chance to initialize before - // continuing. - while (pwmSystem->readLoopTiming(status) == 0) delayTicks(1); - - if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) { - // TODO: char err[128]; - // TODO: sprintf(err, "DIO LoopTiming: %d, expecting: %lu\n", - // digitalModules[port->module-1]->readLoopTiming(status), - // kExpectedLoopTiming); - *status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error - } - - // Calculate the length, in ms, of one DIO loop - double loopTime = pwmSystem->readLoopTiming(status) / - (kSystemClockTicksPerMicrosecond * 1e3); - - pwmSystem->writeConfig_Period((uint16_t)(kDefaultPwmPeriod / loopTime + .5), - status); - uint16_t minHigh = (uint16_t)( - (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + .5); - pwmSystem->writeConfig_MinHigh(minHigh, status); - // printf("MinHigh: %d\n", minHigh); - // Ensure that PWM output values are set to OFF - for (uint32_t pwm_index = 0; pwm_index < kPwmPins; pwm_index++) { - // Initialize port structure - DigitalPort digital_port; - digital_port.port.pin = pwm_index; - - setPWM(&digital_port, kPwmDisabled, status); - setPWMPeriodScale(&digital_port, 3, status); // Set all to 4x by default. - } - - digitalSystemsInitialized = true; -} - -/** - * Create a new instance of a digital port. - */ -void* initializeDigitalPort(void* port_pointer, int32_t* status) { - initializeDigital(status); - Port* port = (Port*)port_pointer; - - // Initialize port structure - DigitalPort* digital_port = new DigitalPort(); - digital_port->port = *port; - - return digital_port; -} - -void freeDigitalPort(void* digital_port_pointer) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - delete port; -} - -bool checkPWMChannel(void* digital_port_pointer) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - return port->port.pin < kPwmPins; -} - -bool checkRelayChannel(void* digital_port_pointer) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - return port->port.pin < kRelayPins; -} - -/** - * Check a port to make sure that it is not NULL and is a valid PWM port. - * - * Sets the status to contain the appropriate error. - * - * @return true if the port passed validation. - */ -static bool verifyPWMChannel(DigitalPort* port, int32_t* status) { - if (port == NULL) { - *status = NULL_PARAMETER; - return false; - } else if (!checkPWMChannel(port)) { - *status = PARAMETER_OUT_OF_RANGE; - return false; - } else { - return true; - } -} - -/** - * Check a port to make sure that it is not NULL and is a valid Relay port. - * - * Sets the status to contain the appropriate error. - * - * @return true if the port passed validation. - */ -static bool verifyRelayChannel(DigitalPort* port, int32_t* status) { - if (port == NULL) { - *status = NULL_PARAMETER; - return false; - } else if (!checkRelayChannel(port)) { - *status = PARAMETER_OUT_OF_RANGE; - return false; - } else { - return true; - } -} - -/** - * Map DIO pin numbers from their physical number (10 to 26) to their position - * in the bit field. - */ -uint32_t remapMXPChannel(uint32_t pin) { return pin - 10; } - -uint32_t remapMXPPWMChannel(uint32_t pin) { - if (pin < 14) { - return pin - 10; // first block of 4 pwms (MXP 0-3) - } else { - return pin - 6; // block of PWMs after SPI - } -} - -/** - * Set a PWM channel to the desired value. The values range from 0 to 255 and - * the period is controlled - * by the PWM Period and MinHigh registers. - * - * @param channel The PWM channel to set. - * @param value The PWM value to set. - */ -void setPWM(void* digital_port_pointer, unsigned short value, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyPWMChannel(port, status)) { - return; - } - - if (port->port.pin < tPWM::kNumHdrRegisters) { - pwmSystem->writeHdr(port->port.pin, value, status); - } else { - pwmSystem->writeMXP(port->port.pin - tPWM::kNumHdrRegisters, value, status); - } -} - -/** - * Get a value from a PWM channel. The values range from 0 to 255. - * - * @param channel The PWM channel to read from. - * @return The raw PWM value. - */ -unsigned short getPWM(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyPWMChannel(port, status)) { - return 0; - } - - if (port->port.pin < tPWM::kNumHdrRegisters) { - return pwmSystem->readHdr(port->port.pin, status); - } else { - return pwmSystem->readMXP(port->port.pin - tPWM::kNumHdrRegisters, status); - } -} - -void latchPWMZero(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyPWMChannel(port, status)) { - return; - } - - pwmSystem->writeZeroLatch(port->port.pin, true, status); - pwmSystem->writeZeroLatch(port->port.pin, false, status); -} - -/** - * Set how how often the PWM signal is squelched, thus scaling the period. - * - * @param channel The PWM channel to configure. - * @param squelchMask The 2-bit mask of outputs to squelch. - */ -void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, - int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyPWMChannel(port, status)) { - return; - } - - if (port->port.pin < tPWM::kNumPeriodScaleHdrElements) { - pwmSystem->writePeriodScaleHdr(port->port.pin, squelchMask, status); - } else { - pwmSystem->writePeriodScaleMXP( - port->port.pin - tPWM::kNumPeriodScaleHdrElements, squelchMask, status); - } -} - -/** - * Allocate a DO PWM Generator. - * Allocate PWM generators so that they are not accidentally reused. - * - * @return PWM Generator refnum - */ -void* allocatePWM(int32_t* status) { - return (void*)DO_PWMGenerators->Allocate("DO_PWM"); -} - -/** - * Free the resource associated with a DO PWM generator. - * - * @param pwmGenerator The pwmGen to free that was allocated with - * AllocateDO_PWM() - */ -void freePWM(void* pwmGenerator, int32_t* status) { - uint32_t id = (uint32_t)pwmGenerator; - if (id == ~0ul) return; - DO_PWMGenerators->Free(id); -} - -/** - * Change the frequency of the DO PWM generator. - * - * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is - * logarithmic. - * - * @param rate The frequency to output all digital output PWM signals. - */ -void setPWMRate(double rate, int32_t* status) { - // Currently rounding in the log rate domain... heavy weight toward picking a - // higher freq. - // TODO: Round in the linear rate domain. - uint8_t pwmPeriodPower = (uint8_t)( - log(1.0 / (pwmSystem->readLoopTiming(status) * 0.25E-6 * rate)) / - log(2.0) + - 0.5); - digitalSystem->writePWMPeriodPower(pwmPeriodPower, status); -} - -/** - * Configure the duty-cycle of the PWM generator - * - * @param pwmGenerator The generator index reserved by AllocateDO_PWM() - * @param dutyCycle The percent duty cycle to output [0..1]. - */ -void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t* status) { - uint32_t id = (uint32_t)pwmGenerator; - if (id == ~0ul) return; - if (dutyCycle > 1.0) dutyCycle = 1.0; - if (dutyCycle < 0.0) dutyCycle = 0.0; - float rawDutyCycle = 256.0 * dutyCycle; - if (rawDutyCycle > 255.5) rawDutyCycle = 255.5; - { - std::lock_guard sync(digitalPwmMutex); - uint8_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status); - if (pwmPeriodPower < 4) { - // The resolution of the duty cycle drops close to the highest - // frequencies. - rawDutyCycle = rawDutyCycle / pow(2.0, 4 - pwmPeriodPower); - } - if (id < 4) - digitalSystem->writePWMDutyCycleA(id, (uint8_t)rawDutyCycle, status); - else - digitalSystem->writePWMDutyCycleB(id - 4, (uint8_t)rawDutyCycle, status); - } -} - -/** - * Configure which DO channel the PWM signal is output on - * - * @param pwmGenerator The generator index reserved by AllocateDO_PWM() - * @param channel The Digital Output channel to output on - */ -void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t* status) { - uint32_t id = (uint32_t)pwmGenerator; - if (id > 5) return; - digitalSystem->writePWMOutputSelect(id, pin, status); -} - -/** - * Set the state of a relay. - * Set the state of a relay output to be forward. Relays have two outputs and - * each is - * independently set to 0v or 12v. - */ -void setRelayForward(void* digital_port_pointer, bool on, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyRelayChannel(port, status)) { - return; - } - - { - std::lock_guard sync(digitalRelayMutex); - uint8_t forwardRelays = relaySystem->readValue_Forward(status); - if (on) - forwardRelays |= 1 << port->port.pin; - else - forwardRelays &= ~(1 << port->port.pin); - relaySystem->writeValue_Forward(forwardRelays, status); - } -} - -/** - * Set the state of a relay. - * Set the state of a relay output to be reverse. Relays have two outputs and - * each is - * independently set to 0v or 12v. - */ -void setRelayReverse(void* digital_port_pointer, bool on, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyRelayChannel(port, status)) { - return; - } - - { - std::lock_guard sync(digitalRelayMutex); - uint8_t reverseRelays = relaySystem->readValue_Reverse(status); - if (on) - reverseRelays |= 1 << port->port.pin; - else - reverseRelays &= ~(1 << port->port.pin); - relaySystem->writeValue_Reverse(reverseRelays, status); - } -} - -/** - * Get the current state of the forward relay channel - */ -bool getRelayForward(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyRelayChannel(port, status)) { - return false; - } - - uint8_t forwardRelays = relaySystem->readValue_Forward(status); - return (forwardRelays & (1 << port->port.pin)) != 0; -} - -/** - * Get the current state of the reverse relay channel - */ -bool getRelayReverse(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyRelayChannel(port, status)) { - return false; - } - - uint8_t reverseRelays = relaySystem->readValue_Reverse(status); - return (reverseRelays & (1 << port->port.pin)) != 0; -} - -/** - * Allocate Digital I/O channels. - * Allocate channels so that they are not accidently reused. Also the direction - * is set at the time of the allocation. - * - * @param channel The Digital I/O channel - * @param input If true open as input; if false open as output - * @return Was successfully allocated - */ -bool allocateDIO(void* digital_port_pointer, bool input, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - char buf[64]; - snprintf(buf, 64, "DIO %d", port->port.pin); - if (DIOChannels->Allocate(port->port.pin, buf) == ~0ul) { - *status = RESOURCE_IS_ALLOCATED; - return false; - } - - { - std::lock_guard sync(digitalDIOMutex); - - tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status); - - if (port->port.pin < kNumHeaders) { - uint32_t bitToSet = 1 << port->port.pin; - if (input) { - outputEnable.Headers = - outputEnable.Headers & (~bitToSet); // clear the bit for read - } else { - outputEnable.Headers = - outputEnable.Headers | bitToSet; // set the bit for write - } - } else { - uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); - - // Disable special functions on this pin - short specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, - status); - - if (input) { - outputEnable.MXP = - outputEnable.MXP & (~bitToSet); // clear the bit for read - } else { - outputEnable.MXP = - outputEnable.MXP | bitToSet; // set the bit for write - } - } - - digitalSystem->writeOutputEnable(outputEnable, status); - } - return true; -} - -bool allocatePWMChannel(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!verifyPWMChannel(port, status)) { - return false; - } - - char buf[64]; - snprintf(buf, 64, "PWM %d", port->port.pin); - if (PWMChannels->Allocate(port->port.pin, buf) == ~0ul) { - *status = RESOURCE_IS_ALLOCATED; - return false; - } - - if (port->port.pin > tPWM::kNumHdrRegisters - 1) { - snprintf(buf, 64, "PWM %d and DIO %d", port->port.pin, - remapMXPPWMChannel(port->port.pin) + 10); - if (DIOChannels->Allocate(remapMXPPWMChannel(port->port.pin) + 10, buf) == - ~0ul) - return false; - uint32_t bitToSet = 1 << remapMXPPWMChannel(port->port.pin); - short specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet, - status); - } - return true; -} - -void freePWMChannel(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!port) return; - if (!verifyPWMChannel(port, status)) { - return; - } - - PWMChannels->Free(port->port.pin); - if (port->port.pin > tPWM::kNumHdrRegisters - 1) { - DIOChannels->Free(remapMXPPWMChannel(port->port.pin) + 10); - uint32_t bitToUnset = 1 << remapMXPPWMChannel(port->port.pin); - short specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset, - status); - } -} - -/** - * Free the resource associated with a digital I/O channel. - * - * @param channel The Digital I/O channel to free - */ -void freeDIO(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (!port) return; - DIOChannels->Free(port->port.pin); -} - -/** - * Write a digital I/O bit to the FPGA. - * Set a single value on a digital I/O channel. - * - * @param channel The Digital I/O channel - * @param value The state to set the digital channel (if it is configured as an - * output) - */ -void setDIO(void* digital_port_pointer, short value, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - if (value != 0 && value != 1) { - if (value != 0) value = 1; - } - { - std::lock_guard sync(digitalDIOMutex); - tDIO::tDO currentDIO = digitalSystem->readDO(status); - - if (port->port.pin < kNumHeaders) { - if (value == 0) { - currentDIO.Headers = currentDIO.Headers & ~(1 << port->port.pin); - } else if (value == 1) { - currentDIO.Headers = currentDIO.Headers | (1 << port->port.pin); - } - } else { - if (value == 0) { - currentDIO.MXP = - currentDIO.MXP & ~(1 << remapMXPChannel(port->port.pin)); - } else if (value == 1) { - currentDIO.MXP = - currentDIO.MXP | (1 << remapMXPChannel(port->port.pin)); - } - - uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); - short specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, - status); - } - digitalSystem->writeDO(currentDIO, status); - } -} - -/** - * Read a digital I/O bit from the FPGA. - * Get a single value from a digital I/O channel. - * - * @param channel The digital I/O channel - * @return The state of the specified channel - */ -bool getDIO(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - tDIO::tDI currentDIO = digitalSystem->readDI(status); - // Shift 00000001 over channel-1 places. - // AND it against the currentDIO - // if it == 0, then return false - // else return true - - if (port->port.pin < kNumHeaders) { - return ((currentDIO.Headers >> port->port.pin) & 1) != 0; - } else { - // Disable special functions - uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); - short specialFunctions = - digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, - status); - - return ((currentDIO.MXP >> remapMXPChannel(port->port.pin)) & 1) != 0; - } -} - -/** - * Read the direction of a the Digital I/O lines - * A 1 bit means output and a 0 bit means input. - * - * @param channel The digital I/O channel - * @return The direction of the specified channel - */ -bool getDIODirection(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - tDIO::tOutputEnable currentOutputEnable = - digitalSystem->readOutputEnable(status); - // Shift 00000001 over port->port.pin-1 places. - // AND it against the currentOutputEnable - // if it == 0, then return false - // else return true - - if (port->port.pin < kNumHeaders) { - return ((currentOutputEnable.Headers >> port->port.pin) & 1) != 0; - } else { - return ((currentOutputEnable.MXP >> remapMXPChannel(port->port.pin)) & 1) != - 0; - } -} - -/** - * Generate a single pulse. - * Write a pulse to the specified digital output channel. There can only be a - * single pulse going at any time. - * - * @param channel The Digital Output channel that the pulse should be output on - * @param pulseLength The active length of the pulse (in seconds) - */ -void pulse(void* digital_port_pointer, double pulseLength, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - tDIO::tPulse pulse; - - if (port->port.pin < kNumHeaders) { - pulse.Headers = 1 << port->port.pin; - } else { - pulse.MXP = 1 << remapMXPChannel(port->port.pin); - } - - digitalSystem->writePulseLength( - (uint8_t)(1.0e9 * pulseLength / (pwmSystem->readLoopTiming(status) * 25)), - status); - digitalSystem->writePulse(pulse, status); -} - -/** - * Check a DIO line to see if it is currently generating a pulse. - * - * @return A pulse is in progress - */ -bool isPulsing(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); - - if (port->port.pin < kNumHeaders) { - return (pulseRegister.Headers & (1 << port->port.pin)) != 0; - } else { - return (pulseRegister.MXP & (1 << remapMXPChannel(port->port.pin))) != 0; - } -} - -/** - * Check if any DIO line is currently generating a pulse. - * - * @return A pulse on some line is in progress - */ -bool isAnyPulsing(int32_t* status) { - tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); - return pulseRegister.Headers != 0 && pulseRegister.MXP != 0; -} - -/** - * Write the filter index from the FPGA. - * Set the filter index used to filter out short pulses. - * - * @param digital_port_pointer The digital I/O channel - * @param filter_index The filter index. Must be in the range 0 - 3, - * where 0 means "none" and 1 - 3 means filter # filter_index - 1. - */ -void setFilterSelect(void* digital_port_pointer, int filter_index, - int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - - std::lock_guard sync(digitalDIOMutex); - if (port->port.pin < kNumHeaders) { - digitalSystem->writeFilterSelectHdr(port->port.pin, filter_index, status); - } else { - digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->port.pin), - filter_index, status); - } -} - -/** - * Read the filter index from the FPGA. - * Get the filter index used to filter out short pulses. - * - * @param digital_port_pointer The digital I/O channel - * @return filter_index The filter index. Must be in the range 0 - 3, - * where 0 means "none" and 1 - 3 means filter # filter_index - 1. - */ -int getFilterSelect(void* digital_port_pointer, int32_t* status) { - DigitalPort* port = (DigitalPort*)digital_port_pointer; - - std::lock_guard sync(digitalDIOMutex); - if (port->port.pin < kNumHeaders) { - return digitalSystem->readFilterSelectHdr(port->port.pin, status); - } else { - return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->port.pin), - status); - } -} - -/** - * Set the filter period for the specified filter index. - * - * Set the filter period in FPGA cycles. Even though there are 2 different - * filter index domains (MXP vs HDR), ignore that distinction for now since it - * compilicates the interface. That can be changed later. - * - * @param filter_index The filter index, 0 - 2. - * @param value The number of cycles that the signal must not transition to be - * counted as a transition. - */ -void setFilterPeriod(int filter_index, uint32_t value, int32_t* status) { - std::lock_guard sync(digitalDIOMutex); - digitalSystem->writeFilterPeriodHdr(filter_index, value, status); - if (*status == 0) { - digitalSystem->writeFilterPeriodMXP(filter_index, value, status); - } -} - -/** - * Get the filter period for the specified filter index. - * - * Get the filter period in FPGA cycles. Even though there are 2 different - * filter index domains (MXP vs HDR), ignore that distinction for now since it - * compilicates the interface. Set status to NiFpga_Status_SoftwareFault if the - * filter values miss-match. - * - * @param filter_index The filter index, 0 - 2. - * @param value The number of cycles that the signal must not transition to be - * counted as a transition. - */ -uint32_t getFilterPeriod(int filter_index, int32_t* status) { - uint32_t hdr_period = 0; - uint32_t mxp_period = 0; - { - std::lock_guard sync(digitalDIOMutex); - hdr_period = digitalSystem->readFilterPeriodHdr(filter_index, status); - if (*status == 0) { - mxp_period = digitalSystem->readFilterPeriodMXP(filter_index, status); - } - } - if (hdr_period != mxp_period) { - *status = NiFpga_Status_SoftwareFault; - return -1; - } - return hdr_period; -} - -struct counter_t { - tCounter* counter; - uint32_t index; -}; -typedef struct counter_t Counter; - -static hal::Resource* counters = NULL; - -void* initializeCounter(Mode mode, uint32_t* index, int32_t* status) { - hal::Resource::CreateResourceObject(&counters, tCounter::kNumSystems); - *index = counters->Allocate("Counter"); - if (*index == ~0ul) { - *status = NO_AVAILABLE_RESOURCES; - return NULL; - } - Counter* counter = new Counter(); - counter->counter = tCounter::create(*index, status); - counter->counter->writeConfig_Mode(mode, status); - counter->counter->writeTimerConfig_AverageSize(1, status); - counter->index = *index; - return counter; -} - -void freeCounter(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - if (!counter) return; - delete counter->counter; - counters->Free(counter->index); -} - -void setCounterAverageSize(void* counter_pointer, int32_t size, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeTimerConfig_AverageSize(size, status); -} - -/** - * remap the digital source pin and set the module. - * If it's an analog trigger, determine the module from the high order routing - * channel else do normal digital input remapping based on pin number (MXP) - */ -extern "C++" void remapDigitalSource(bool analogTrigger, uint32_t& pin, - uint8_t& module) { - if (analogTrigger) { - module = pin >> 4; - } else { - if (pin >= kNumHeaders) { - pin = remapMXPChannel(pin); - module = 1; - } else { - module = 0; - } - } -} - -/** - * Set the source object that causes the counter to count up. - * Set the up counting DigitalSource. - */ -void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - - uint8_t module; - - remapDigitalSource(analogTrigger, pin, module); - - counter->counter->writeConfig_UpSource_Module(module, status); - counter->counter->writeConfig_UpSource_Channel(pin, status); - counter->counter->writeConfig_UpSource_AnalogTrigger(analogTrigger, status); - - if (counter->counter->readConfig_Mode(status) == kTwoPulse || - counter->counter->readConfig_Mode(status) == kExternalDirection) { - setCounterUpSourceEdge(counter_pointer, true, false, status); - } - counter->counter->strobeReset(status); -} - -/** - * Set the edge sensitivity on an up counting source. - * Set the up source to either detect rising edges or falling edges. - */ -void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, - bool fallingEdge, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_UpRisingEdge(risingEdge, status); - counter->counter->writeConfig_UpFallingEdge(fallingEdge, status); -} - -/** - * Disable the up counting source to the counter. - */ -void clearCounterUpSource(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_UpFallingEdge(false, status); - counter->counter->writeConfig_UpRisingEdge(false, status); - // Index 0 of digital is always 0. - counter->counter->writeConfig_UpSource_Channel(0, status); - counter->counter->writeConfig_UpSource_AnalogTrigger(false, status); -} - -/** - * Set the source object that causes the counter to count down. - * Set the down counting DigitalSource. - */ -void setCounterDownSource(void* counter_pointer, uint32_t pin, - bool analogTrigger, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - unsigned char mode = counter->counter->readConfig_Mode(status); - if (mode != kTwoPulse && mode != kExternalDirection) { - // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only - // supports DownSource in TwoPulse and ExternalDirection modes."); - *status = PARAMETER_OUT_OF_RANGE; - return; - } - - uint8_t module; - - remapDigitalSource(analogTrigger, pin, module); - - counter->counter->writeConfig_DownSource_Module(module, status); - counter->counter->writeConfig_DownSource_Channel(pin, status); - counter->counter->writeConfig_DownSource_AnalogTrigger(analogTrigger, status); - - setCounterDownSourceEdge(counter_pointer, true, false, status); - counter->counter->strobeReset(status); -} - -/** - * Set the edge sensitivity on a down counting source. - * Set the down source to either detect rising edges or falling edges. - */ -void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, - bool fallingEdge, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_DownRisingEdge(risingEdge, status); - counter->counter->writeConfig_DownFallingEdge(fallingEdge, status); -} - -/** - * Disable the down counting source to the counter. - */ -void clearCounterDownSource(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_DownFallingEdge(false, status); - counter->counter->writeConfig_DownRisingEdge(false, status); - // Index 0 of digital is always 0. - counter->counter->writeConfig_DownSource_Channel(0, status); - counter->counter->writeConfig_DownSource_AnalogTrigger(false, status); -} - -/** - * Set standard up / down counting mode on this counter. - * Up and down counts are sourced independently from two inputs. - */ -void setCounterUpDownMode(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_Mode(kTwoPulse, status); -} - -/** - * Set external direction mode on this counter. - * Counts are sourced on the Up counter input. - * The Down counter input represents the direction to count. - */ -void setCounterExternalDirectionMode(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_Mode(kExternalDirection, status); -} - -/** - * Set Semi-period mode on this counter. - * Counts up on both rising and falling edges. - */ -void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_Mode(kSemiperiod, status); - counter->counter->writeConfig_UpRisingEdge(highSemiPeriod, status); - setCounterUpdateWhenEmpty(counter_pointer, false, status); -} - -/** - * Configure the counter to count in up or down based on the length of the input - * pulse. - * This mode is most useful for direction sensitive gear tooth sensors. - * @param threshold The pulse length beyond which the counter counts the - * opposite direction. Units are seconds. - */ -void setCounterPulseLengthMode(void* counter_pointer, double threshold, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeConfig_Mode(kPulseLength, status); - counter->counter->writeConfig_PulseLengthThreshold( - (uint32_t)(threshold * 1.0e6) * kSystemClockTicksPerMicrosecond, status); -} - -/** - * Get the Samples to Average which specifies the number of samples of the timer - * to - * average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. - * @return SamplesToAverage The number of samples being averaged (from 1 to 127) - */ -int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - return counter->counter->readTimerConfig_AverageSize(status); -} - -/** - * Set the Samples to Average which specifies the number of samples of the timer - * to average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. - * @param samplesToAverage The number of samples to average from 1 to 127. - */ -void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - if (samplesToAverage < 1 || samplesToAverage > 127) { - *status = PARAMETER_OUT_OF_RANGE; - } - counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status); -} - -/** - * Reset the Counter to zero. - * Set the counter value to zero. This doesn't effect the running state of the - * counter, just sets the current value to zero. - */ -void resetCounter(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->strobeReset(status); -} - -/** - * Read the current counter value. - * Read the value at this instant. It may still be running, so it reflects the - * current value. Next time it is read, it might have a different value. - */ -int32_t getCounter(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - int32_t value = counter->counter->readOutput_Value(status); - return value; -} - -/* - * Get the Period of the most recent count. - * Returns the time interval of the most recent count. This can be used for - * velocity calculations to determine shaft speed. - * @returns The period of the last two pulses in units of seconds. - */ -double getCounterPeriod(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - tCounter::tTimerOutput output = counter->counter->readTimerOutput(status); - double period; - if (output.Stalled) { - // Return infinity - double zero = 0.0; - period = 1.0 / zero; - } else { - // output.Period is a fixed point number that counts by 2 (24 bits, 25 - // integer bits) - period = (double)(output.Period << 1) / (double)output.Count; - } - return period * 2.5e-8; // result * timebase (currently 40ns) -} - -/** - * Set the maximum period where the device is still considered "moving". - * Sets the maximum period where the device is considered moving. This value is - * used to determine the "stopped" state of the counter using the GetStopped - * method. - * @param maxPeriod The maximum period where the counted device is considered - * moving in seconds. - */ -void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 4.0e8), - status); -} - -/** - * Select whether you want to continue updating the event timer output when - * there are no samples captured. The output of the event timer has a buffer of - * periods that are averaged and posted to a register on the FPGA. When the - * timer detects that the event source has stopped (based on the MaxPeriod) the - * buffer of samples to be averaged is emptied. If you enable the update when - * empty, you will be notified of the stopped source and the event time will - * report 0 samples. If you disable update when empty, the most recent average - * will remain on the output until a new sample is acquired. You will never see - * 0 samples output (except when there have been no events since an FPGA reset) - * and you will likely not see the stopped bit become true (since it is updated - * at the end of an average and there are no samples to average). - */ -void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - counter->counter->writeTimerConfig_UpdateWhenEmpty(enabled, status); -} - -/** - * Determine if the clock is stopped. - * Determine if the clocked input is stopped based on the MaxPeriod value set - * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the - * device (and counter) are assumed to be stopped and it returns true. - * @return Returns true if the most recent counter period exceeds the MaxPeriod - * value set by SetMaxPeriod. - */ -bool getCounterStopped(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - return counter->counter->readTimerOutput_Stalled(status); -} - -/** - * The last direction the counter value changed. - * @return The last direction the counter value changed. - */ -bool getCounterDirection(void* counter_pointer, int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - bool value = counter->counter->readOutput_Direction(status); - return value; -} - -/** - * Set the Counter to return reversed sensing on the direction. - * This allows counters to change the direction they are counting in the case of - * 1X and 2X quadrature encoding only. Any other counter mode isn't supported. - * @param reverseDirection true if the value counted should be negated. - */ -void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, - int32_t* status) { - Counter* counter = (Counter*)counter_pointer; - if (counter->counter->readConfig_Mode(status) == kExternalDirection) { - if (reverseDirection) - setCounterDownSourceEdge(counter_pointer, true, true, status); - else - setCounterDownSourceEdge(counter_pointer, false, true, status); - } -} - -struct encoder_t { - tEncoder* encoder; - uint32_t index; -}; -typedef struct encoder_t Encoder; - -static const double DECODING_SCALING_FACTOR = 0.25; -static hal::Resource* quadEncoders = NULL; - -void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, - bool port_a_analog_trigger, uint8_t port_b_module, - uint32_t port_b_pin, bool port_b_analog_trigger, - bool reverseDirection, int32_t* index, - int32_t* status) { - // Initialize encoder structure - Encoder* encoder = new Encoder(); - - remapDigitalSource(port_a_analog_trigger, port_a_pin, port_a_module); - remapDigitalSource(port_b_analog_trigger, port_b_pin, port_b_module); - - hal::Resource::CreateResourceObject(&quadEncoders, tEncoder::kNumSystems); - encoder->index = quadEncoders->Allocate("4X Encoder"); - *index = encoder->index; - // TODO: if (index == ~0ul) { CloneError(quadEncoders); return; } - encoder->encoder = tEncoder::create(encoder->index, status); - encoder->encoder->writeConfig_ASource_Module(port_a_module, status); - encoder->encoder->writeConfig_ASource_Channel(port_a_pin, status); - encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger, - status); - encoder->encoder->writeConfig_BSource_Module(port_b_module, status); - encoder->encoder->writeConfig_BSource_Channel(port_b_pin, status); - encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger, - status); - encoder->encoder->strobeReset(status); - encoder->encoder->writeConfig_Reverse(reverseDirection, status); - encoder->encoder->writeTimerConfig_AverageSize(4, status); - - return encoder; -} - -void freeEncoder(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - if (!encoder) return; - quadEncoders->Free(encoder->index); - delete encoder->encoder; -} - -/** - * Reset the Encoder distance to zero. - * Resets the current count to zero on the encoder. - */ -void resetEncoder(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - encoder->encoder->strobeReset(status); -} - -/** - * Gets the raw value from the encoder. - * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale - * factor. - * @return Current raw count from the encoder - */ -int32_t getEncoder(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - return encoder->encoder->readOutput_Value(status); -} - -/** - * Returns the period of the most recent pulse. - * Returns the period of the most recent Encoder pulse in seconds. - * This method compenstates for the decoding type. - * - * @deprecated Use GetRate() in favor of this method. This returns unscaled - * periods and GetRate() scales using value from SetDistancePerPulse(). - * - * @return Period in seconds of the most recent pulse. - */ -double getEncoderPeriod(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status); - double value; - if (output.Stalled) { - // Return infinity - double zero = 0.0; - value = 1.0 / zero; - } else { - // output.Period is a fixed point number that counts by 2 (24 bits, 25 - // integer bits) - value = (double)(output.Period << 1) / (double)output.Count; - } - double measuredPeriod = value * 2.5e-8; - return measuredPeriod / DECODING_SCALING_FACTOR; -} - -/** - * Sets the maximum period for stopped detection. - * Sets the value that represents the maximum period of the Encoder before it - * will assume that the attached device is stopped. This timeout allows users - * to determine if the wheels or other shaft has stopped rotating. - * This method compensates for the decoding type. - * - * @deprecated Use SetMinRate() in favor of this method. This takes unscaled - * periods and SetMinRate() scales using value from SetDistancePerPulse(). - * - * @param maxPeriod The maximum time between rising and falling edges before the - * FPGA will - * report the device stopped. This is expressed in seconds. - */ -void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, - int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - encoder->encoder->writeTimerConfig_StallPeriod( - (uint32_t)(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR), status); -} - -/** - * Determine if the encoder is stopped. - * Using the MaxPeriod value, a boolean is returned that is true if the encoder - * is considered stopped and false if it is still moving. A stopped encoder is - * one where the most recent pulse width exceeds the MaxPeriod. - * @return True if the encoder is considered stopped. - */ -bool getEncoderStopped(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - return encoder->encoder->readTimerOutput_Stalled(status) != 0; -} - -/** - * The last direction the encoder value changed. - * @return The last direction the encoder value changed. - */ -bool getEncoderDirection(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - return encoder->encoder->readOutput_Direction(status); -} - -/** - * Set the direction sensing for this encoder. - * This sets the direction sensing on the encoder so that it could count in the - * correct software direction regardless of the mounting. - * @param reverseDirection true if the encoder direction should be reversed - */ -void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, - int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - encoder->encoder->writeConfig_Reverse(reverseDirection, status); -} - -/** - * Set the Samples to Average which specifies the number of samples of the timer - * to average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. - * @param samplesToAverage The number of samples to average from 1 to 127. - */ -void setEncoderSamplesToAverage(void* encoder_pointer, - uint32_t samplesToAverage, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - if (samplesToAverage < 1 || samplesToAverage > 127) { - *status = PARAMETER_OUT_OF_RANGE; - } - encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status); -} - -/** - * Get the Samples to Average which specifies the number of samples of the timer - * to average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. - * @return SamplesToAverage The number of samples being averaged (from 1 to 127) - */ -uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - return encoder->encoder->readTimerConfig_AverageSize(status); -} - -/** - * Set an index source for an encoder, which is an input that resets the - * encoder's count. - */ -void setEncoderIndexSource(void* encoder_pointer, uint32_t pin, - bool analogTrigger, bool activeHigh, - bool edgeSensitive, int32_t* status) { - Encoder* encoder = (Encoder*)encoder_pointer; - encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status); - encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status); - encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger, - status); - encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status); - encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status); -} - -/** - * Get the loop timing of the PWM system - * - * @return The loop time - */ -uint16_t getLoopTiming(int32_t* status) { - return pwmSystem->readLoopTiming(status); -} - -/* - * Initialize the spi port. Opens the port if necessary and saves the handle. - * If opening the MXP port, also sets up the pin functions appropriately - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - */ -void spiInitialize(uint8_t port, int32_t* status) { - if (spiSystem == NULL) spiSystem = tSPI::create(status); - if (spiGetHandle(port) != 0) return; - switch (port) { - case 0: - spiSetHandle(0, spilib_open("/dev/spidev0.0")); - break; - case 1: - spiSetHandle(1, spilib_open("/dev/spidev0.1")); - break; - case 2: - spiSetHandle(2, spilib_open("/dev/spidev0.2")); - break; - case 3: - spiSetHandle(3, spilib_open("/dev/spidev0.3")); - break; - case 4: - initializeDigital(status); - if (!allocateDIO(getPort(14), false, status)) { - printf("Failed to allocate DIO 14\n"); - return; - } - if (!allocateDIO(getPort(15), false, status)) { - printf("Failed to allocate DIO 15\n"); - return; - } - if (!allocateDIO(getPort(16), true, status)) { - printf("Failed to allocate DIO 16\n"); - return; - } - if (!allocateDIO(getPort(17), false, status)) { - printf("Failed to allocate DIO 17\n"); - return; - } - digitalSystem->writeEnableMXPSpecialFunction( - digitalSystem->readEnableMXPSpecialFunction(status) | 0x00F0, status); - spiSetHandle(4, spilib_open("/dev/spidev1.0")); - break; - default: - break; - } - return; -} - -/** - * Generic transaction. - * - * This is a lower-level interface to the spi hardware giving you more control - * over each transaction. - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @param dataToSend Buffer of data to send as part of the transaction. - * @param dataReceived Buffer to read data into. - * @param size Number of bytes to transfer. [0..7] - * @return Number of bytes transferred, -1 for error - */ -int32_t spiTransaction(uint8_t port, uint8_t* dataToSend, uint8_t* dataReceived, - uint8_t size) { - std::lock_guard sync(spiGetSemaphore(port)); - return spilib_writeread(spiGetHandle(port), (const char*)dataToSend, - (char*)dataReceived, (int32_t)size); -} - -/** - * Execute a write transaction with the device. - * - * Write to a device and wait until the transaction is complete. - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @param datToSend The data to write to the register on the device. - * @param sendSize The number of bytes to be written - * @return The number of bytes written. -1 for an error - */ -int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize) { - std::lock_guard sync(spiGetSemaphore(port)); - return spilib_write(spiGetHandle(port), (const char*)dataToSend, - (int32_t)sendSize); -} - -/** - * Execute a read from the device. - * - * This method does not write any data out to the device - * Most spi devices will require a register address to be written before - * they begin returning data - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @param buffer A pointer to the array of bytes to store the data read from the - * device. - * @param count The number of bytes to read in the transaction. [1..7] - * @return Number of bytes read. -1 for error. - */ -int32_t spiRead(uint8_t port, uint8_t* buffer, uint8_t count) { - std::lock_guard sync(spiGetSemaphore(port)); - return spilib_read(spiGetHandle(port), (char*)buffer, (int32_t)count); -} - -/** - * Close the SPI port - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - */ -void spiClose(uint8_t port) { - std::lock_guard sync(spiGetSemaphore(port)); - if (spiAccumulators[port]) { - int32_t status = 0; - spiFreeAccumulator(port, &status); - } - spilib_close(spiGetHandle(port)); - spiSetHandle(port, 0); - return; -} - -/** - * Set the clock speed for the SPI bus. - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @param speed The speed in Hz (0-1MHz) - */ -void spiSetSpeed(uint8_t port, uint32_t speed) { - std::lock_guard sync(spiGetSemaphore(port)); - spilib_setspeed(spiGetHandle(port), speed); -} - -/** - * Set the SPI options - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @param msb_first True to write the MSB first, False for LSB first - * @param sample_on_trailing True to sample on the trailing edge, False to - * sample on the leading edge - * @param clk_idle_high True to set the clock to active low, False to set the - * clock active high - */ -void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, - int clk_idle_high) { - std::lock_guard sync(spiGetSemaphore(port)); - spilib_setopts(spiGetHandle(port), msb_first, sample_on_trailing, - clk_idle_high); -} - -/** - * Set the CS Active high for a SPI port - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - */ -void spiSetChipSelectActiveHigh(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - if (port < 4) { - spiSystem->writeChipSelectActiveHigh_Hdr( - spiSystem->readChipSelectActiveHigh_Hdr(status) | (1 << port), status); - } else { - spiSystem->writeChipSelectActiveHigh_MXP(1, status); - } -} - -/** - * Set the CS Active low for a SPI port - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - */ -void spiSetChipSelectActiveLow(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - if (port < 4) { - spiSystem->writeChipSelectActiveHigh_Hdr( - spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1 << port), status); - } else { - spiSystem->writeChipSelectActiveHigh_MXP(0, status); - } -} - -/** - * Get the stored handle for a SPI port - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @return The stored handle for the SPI port. 0 represents no stored handle. - */ -int32_t spiGetHandle(uint8_t port) { - std::lock_guard sync(spiGetSemaphore(port)); - switch (port) { - case 0: - return m_spiCS0Handle; - case 1: - return m_spiCS1Handle; - case 2: - return m_spiCS2Handle; - case 3: - return m_spiCS3Handle; - case 4: - return m_spiMXPHandle; - default: - return 0; - } -} - -/** - * Set the stored handle for a SPI port - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for - * MXP. - * @param handle The value of the handle for the port. - */ -void spiSetHandle(uint8_t port, int32_t handle) { - std::lock_guard sync(spiGetSemaphore(port)); - switch (port) { - case 0: - m_spiCS0Handle = handle; - break; - case 1: - m_spiCS1Handle = handle; - break; - case 2: - m_spiCS2Handle = handle; - break; - case 3: - m_spiCS3Handle = handle; - break; - case 4: - m_spiMXPHandle = handle; - break; - default: - break; - } -} - -/** - * Get the semaphore for a SPI port - * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @return The semaphore for the SPI port. - */ -extern "C++" priority_recursive_mutex& spiGetSemaphore(uint8_t port) { - if (port < 4) - return spiOnboardSemaphore; - else - return spiMXPSemaphore; -} - -static void spiAccumulatorProcess(uint64_t currentTime, void* param) { - SPIAccumulator* accum = (SPIAccumulator*)param; - - // perform SPI transaction - uint8_t resp_b[4]; - std::lock_guard sync(spiGetSemaphore(accum->port)); - spilib_writeread(spiGetHandle(accum->port), (const char*)accum->cmd, - (char*)resp_b, (int32_t)accum->xfer_size); - - // convert from bytes - uint32_t resp = 0; - if (accum->big_endian) { - for (int i = 0; i < accum->xfer_size; ++i) { - resp <<= 8; - resp |= resp_b[i] & 0xff; - } - } else { - for (int i = accum->xfer_size - 1; i >= 0; --i) { - resp <<= 8; - resp |= resp_b[i] & 0xff; - } - } - - // process response - if ((resp & accum->valid_mask) == accum->valid_value) { - // valid sensor data; extract data field - int32_t data = (int32_t)(resp >> accum->data_shift); - data &= accum->data_max - 1; - // 2s complement conversion if signed MSB is set - if (accum->is_signed && (data & accum->data_msb_mask) != 0) - data -= accum->data_max; - // center offset - data -= accum->center; - // only accumulate if outside deadband - if (data < -accum->deadband || data > accum->deadband) accum->value += data; - ++accum->count; - accum->last_value = data; - } else { - // no data from the sensor; just clear the last value - accum->last_value = 0; - } - - // reschedule timer - accum->triggerTime += accum->period; - // handle timer slip - if (accum->triggerTime < currentTime) - accum->triggerTime = currentTime + accum->period; - int32_t status = 0; - updateNotifierAlarm(accum->notifier, accum->triggerTime, &status); -} - -/** - * Initialize a SPI accumulator. - * - * @param port SPI port - * @param period Time between reads, in us - * @param cmd SPI command to send to request data - * @param xfer_size SPI transfer size, in bytes - * @param valid_mask Mask to apply to received data for validity checking - * @param valid_data After valid_mask is applied, required matching value for - * validity checking - * @param data_shift Bit shift to apply to received data to get actual data - * value - * @param data_size Size (in bits) of data field - * @param is_signed Is data field signed? - * @param big_endian Is device big endian? - */ -void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, - uint8_t xfer_size, uint32_t valid_mask, - uint32_t valid_value, uint8_t data_shift, - uint8_t data_size, bool is_signed, bool big_endian, - int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - if (port > 4) return; - if (!spiAccumulators[port]) spiAccumulators[port] = new SPIAccumulator(); - SPIAccumulator* accum = spiAccumulators[port]; - if (big_endian) { - for (int i = xfer_size - 1; i >= 0; --i) { - accum->cmd[i] = cmd & 0xff; - cmd >>= 8; - } - } else { - accum->cmd[0] = cmd & 0xff; - cmd >>= 8; - accum->cmd[1] = cmd & 0xff; - cmd >>= 8; - accum->cmd[2] = cmd & 0xff; - cmd >>= 8; - accum->cmd[3] = cmd & 0xff; - } - accum->period = period; - accum->xfer_size = xfer_size; - accum->valid_mask = valid_mask; - accum->valid_value = valid_value; - accum->data_shift = data_shift; - accum->data_max = (1 << data_size); - accum->data_msb_mask = (1 << (data_size - 1)); - accum->is_signed = is_signed; - accum->big_endian = big_endian; - if (!accum->notifier) { - accum->notifier = initializeNotifier(spiAccumulatorProcess, accum, status); - accum->triggerTime = getFPGATime(status) + period; - if (*status != 0) return; - updateNotifierAlarm(accum->notifier, accum->triggerTime, status); - } -} - -/** - * Frees a SPI accumulator. - */ -void spiFreeAccumulator(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - cleanNotifier(accum->notifier, status); - delete accum; - spiAccumulators[port] = nullptr; -} - -/** - * Resets the accumulator to zero. - */ -void spiResetAccumulator(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->value = 0; - accum->count = 0; - accum->last_value = 0; -} - -/** - * Set the center value of the accumulator. - * - * The center value is subtracted from each value before it is added to the - * accumulator. This - * is used for the center value of devices like gyros and accelerometers to make - * integration work - * and to take the device offset into account when integrating. - */ -void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->center = center; -} - -/** - * Set the accumulator's deadband. - */ -void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, - int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->deadband = deadband; -} - -/** - * Read the last value read by the accumulator engine. - */ -int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->last_value; -} - -/** - * Read the accumulated value. - * - * @return The 64-bit value accumulated since the last Reset(). - */ -int64_t spiGetAccumulatorValue(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->value; -} - -/** - * Read the number of accumulated values. - * - * Read the count of the accumulated values since the accumulator was last - * Reset(). - * - * @return The number of times samples from the channel were accumulated. - */ -uint32_t spiGetAccumulatorCount(uint8_t port, int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->count; -} - -/** - * Read the average of the accumulated value. - * - * @return The accumulated average value (value / count). - */ -double spiGetAccumulatorAverage(uint8_t port, int32_t* status) { - int64_t value; - uint32_t count; - spiGetAccumulatorOutput(port, &value, &count, status); - if (count == 0) return 0.0; - return ((double)value) / count; -} - -/** - * Read the accumulated value and the number of accumulated values atomically. - * - * This function reads the value and count atomically. - * This can be used for averaging. - * - * @param value Pointer to the 64-bit accumulated output. - * @param count Pointer to the number of accumulation cycles. - */ -void spiGetAccumulatorOutput(uint8_t port, int64_t* value, uint32_t* count, - int32_t* status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - *value = 0; - *count = 0; - return; - } - *value = accum->value; - *count = accum->count; -} - -/* - * Initialize the I2C port. Opens the port if necessary and saves the handle. - * If opening the MXP port, also sets up the pin functions appropriately - * @param port The port to open, 0 for the on-board, 1 for the MXP. - */ -void i2CInitialize(uint8_t port, int32_t* status) { - initializeDigital(status); - - if (port > 1) { - // Set port out of range error here - return; - } - - priority_recursive_mutex& lock = - port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - if (port == 0) { - i2COnboardObjCount++; - if (i2COnBoardHandle > 0) return; - i2COnBoardHandle = i2clib_open("/dev/i2c-2"); - } else if (port == 1) { - i2CMXPObjCount++; - if (i2CMXPHandle > 0) return; - if (!allocateDIO(getPort(24), false, status)) return; - if (!allocateDIO(getPort(25), false, status)) return; - digitalSystem->writeEnableMXPSpecialFunction( - digitalSystem->readEnableMXPSpecialFunction(status) | 0xC000, status); - i2CMXPHandle = i2clib_open("/dev/i2c-1"); - } - return; - } -} - -/** - * Generic transaction. - * - * This is a lower-level interface to the I2C hardware giving you more control - * over each transaction. - * - * @param dataToSend Buffer of data to send as part of the transaction. - * @param sendSize Number of bytes to send as part of the transaction. - * @param dataReceived Buffer to read data into. - * @param receiveSize Number of bytes to read from the device. - * @return The number of bytes read (>= 0) or -1 on transfer abort. - */ -int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, - uint8_t sendSize, uint8_t* dataReceived, - uint8_t receiveSize) { - if (port > 1) { - // Set port out of range error here - return -1; - } - - int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; - priority_recursive_mutex& lock = - port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - - { - std::lock_guard sync(lock); - return i2clib_writeread(handle, deviceAddress, (const char*)dataToSend, - (int32_t)sendSize, (char*)dataReceived, - (int32_t)receiveSize); - } -} - -/** - * Execute a write transaction with the device. - * - * Write a single byte to a register on a device and wait until the - * transaction is complete. - * - * @param registerAddress The address of the register on the device to be - * written. - * @param data The byte to write to the register on the device. - * @return The number of bytes written (>= 0) or -1 on transfer abort. - */ -int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, - uint8_t sendSize) { - if (port > 1) { - // Set port out of range error here - return -1; - } - - int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; - priority_recursive_mutex& lock = - port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - return i2clib_write(handle, deviceAddress, (const char*)dataToSend, - (int32_t)sendSize); - } -} - -/** - * Execute a read transaction with the device. - * - * Read bytes from a device. - * Most I2C devices will auto-increment the register pointer internally allowing - * you to read consecutive registers on a device in a single transaction. - * - * @param registerAddress The register to read first in the transaction. - * @param count The number of bytes to read in the transaction. - * @param buffer A pointer to the array of bytes to store the data read from the - * device. - * @return The number of bytes read (>= 0) or -1 on transfer abort. - */ -int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t* buffer, - uint8_t count) { - if (port > 1) { - // Set port out of range error here - return -1; - } - - int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; - priority_recursive_mutex& lock = - port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - return i2clib_read(handle, deviceAddress, (char*)buffer, (int32_t)count); - } -} - -void i2CClose(uint8_t port) { - if (port > 1) { - // Set port out of range error here - return; - } - priority_recursive_mutex& lock = - port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - if ((port == 0 ? i2COnboardObjCount-- : i2CMXPObjCount--) == 0) { - int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; - i2clib_close(handle); - } - } - return; -} - -} // extern "C" diff --git a/hal/lib/athena/DigitalInternal.cpp b/hal/lib/athena/DigitalInternal.cpp new file mode 100644 index 0000000000..19138afd23 --- /dev/null +++ b/hal/lib/athena/DigitalInternal.cpp @@ -0,0 +1,129 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "DigitalInternal.h" + +#include +#include + +#include + +#include "ChipObject.h" +#include "FRC_NetworkCommunication/LoadOut.h" +#include "HAL/HAL.h" +#include "HAL/Port.h" +#include "HAL/cpp/Resource.h" +#include "HAL/cpp/priority_mutex.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); +namespace hal { +// Create a mutex to protect changes to the DO PWM config +priority_recursive_mutex digitalPwmMutex; + +tDIO* digitalSystem = nullptr; +tRelay* relaySystem = nullptr; +tPWM* pwmSystem = nullptr; +hal::Resource* DIOChannels = nullptr; +hal::Resource* DO_PWMGenerators = nullptr; +hal::Resource* PWMChannels = nullptr; + +bool digitalSystemsInitialized = false; + +/** + * Initialize the digital system. + */ +void initializeDigital(int32_t* status) { + if (digitalSystemsInitialized) return; + + hal::Resource::CreateResourceObject(&DIOChannels, + tDIO::kNumSystems * kDigitalPins); + hal::Resource::CreateResourceObject( + &DO_PWMGenerators, + tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements); + hal::Resource::CreateResourceObject(&PWMChannels, + tPWM::kNumSystems * kPwmPins); + digitalSystem = tDIO::create(status); + + // Relay Setup + relaySystem = tRelay::create(status); + + // Turn off all relay outputs. + relaySystem->writeValue_Forward(0, status); + relaySystem->writeValue_Reverse(0, status); + + // PWM Setup + pwmSystem = tPWM::create(status); + + // Make sure that the 9403 IONode has had a chance to initialize before + // continuing. + while (pwmSystem->readLoopTiming(status) == 0) delayTicks(1); + + if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) { + // TODO: char err[128]; + // TODO: sprintf(err, "DIO LoopTiming: %d, expecting: %lu\n", + // digitalModules[port->module-1]->readLoopTiming(status), + // kExpectedLoopTiming); + *status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error + } + + // Calculate the length, in ms, of one DIO loop + double loopTime = pwmSystem->readLoopTiming(status) / + (kSystemClockTicksPerMicrosecond * 1e3); + + pwmSystem->writeConfig_Period((uint16_t)(kDefaultPwmPeriod / loopTime + .5), + status); + uint16_t minHigh = (uint16_t)( + (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + .5); + pwmSystem->writeConfig_MinHigh(minHigh, status); + // printf("MinHigh: %d\n", minHigh); + // Ensure that PWM output values are set to OFF + for (uint32_t pwm_index = 0; pwm_index < kPwmPins; pwm_index++) { + // Initialize port structure + DigitalPort digital_port; + digital_port.port.pin = pwm_index; + + setPWM(&digital_port, kPwmDisabled, status); + setPWMPeriodScale(&digital_port, 3, status); // Set all to 4x by default. + } + + digitalSystemsInitialized = true; +} + +/** + * Map DIO pin numbers from their physical number (10 to 26) to their position + * in the bit field. + */ +uint32_t remapMXPChannel(uint32_t pin) { return pin - 10; } + +uint32_t remapMXPPWMChannel(uint32_t pin) { + if (pin < 14) { + return pin - 10; // first block of 4 pwms (MXP 0-3) + } else { + return pin - 6; // block of PWMs after SPI + } +} + +/** + * remap the digital source pin and set the module. + * If it's an analog trigger, determine the module from the high order routing + * channel else do normal digital input remapping based on pin number (MXP) + */ +extern "C++" void remapDigitalSource(bool analogTrigger, uint32_t& pin, + uint8_t& module) { + if (analogTrigger) { + module = pin >> 4; + } else { + if (pin >= kNumHeaders) { + pin = remapMXPChannel(pin); + module = 1; + } else { + module = 0; + } + } +} +} diff --git a/hal/lib/athena/DigitalInternal.h b/hal/lib/athena/DigitalInternal.h new file mode 100644 index 0000000000..e9609fa2a4 --- /dev/null +++ b/hal/lib/athena/DigitalInternal.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "ChipObject.h" +#include "HAL/Port.h" +#include "HAL/cpp/Resource.h" + +namespace hal { +constexpr uint32_t kNumHeaders = 10; // Number of non-MXP pins +constexpr uint32_t kDigitalPins = 26; +constexpr uint32_t kPwmPins = 20; + +constexpr uint32_t kExpectedLoopTiming = 40; + +/** + * kDefaultPwmPeriod is in ms + * + * - 20ms periods (50 Hz) are the "safest" setting in that this works for all + * devices + * - 20ms periods seem to be desirable for Vex Motors + * - 20ms periods are the specified period for HS-322HD servos, but work + * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums + * and get hot; by 5.0ms the hum is nearly continuous + * - 10ms periods work well for Victor 884 + * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed + * controllers. Due to the shipping firmware on the Jaguar, we can't run the + * update period less than 5.05 ms. + * + * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period + * scaling is implemented as an output squelch to get longer periods for old + * devices. + */ +constexpr float kDefaultPwmPeriod = 5.05; +/** + * kDefaultPwmCenter is the PWM range center in ms + */ +constexpr float kDefaultPwmCenter = 1.5; +/** + * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint + */ +constexpr int32_t kDefaultPwmStepsDown = 1000; +constexpr int32_t kPwmDisabled = 0; + +// Create a mutex to protect changes to the DO PWM config +extern priority_recursive_mutex digitalPwmMutex; + +extern tDIO* digitalSystem; +extern tRelay* relaySystem; +extern tPWM* pwmSystem; +extern hal::Resource* DIOChannels; +extern hal::Resource* DO_PWMGenerators; +extern hal::Resource* PWMChannels; + +extern bool digitalSystemsInitialized; + +struct DigitalPort { + Port port; + uint32_t PWMGeneratorID; +}; + +void initializeDigital(int32_t* status); +void remapDigitalSource(bool analogTrigger, uint32_t& pin, uint8_t& module); +uint32_t remapMXPPWMChannel(uint32_t pin); +uint32_t remapMXPChannel(uint32_t pin); +} diff --git a/hal/lib/athena/Encoder.cpp b/hal/lib/athena/Encoder.cpp new file mode 100644 index 0000000000..885a48153b --- /dev/null +++ b/hal/lib/athena/Encoder.cpp @@ -0,0 +1,207 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/Encoder.h" + +#include "DigitalInternal.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +extern "C" { +struct encoder_t { + tEncoder* encoder; + uint32_t index; +}; +typedef struct encoder_t Encoder; + +static const double DECODING_SCALING_FACTOR = 0.25; +static hal::Resource* quadEncoders = nullptr; + +void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, + bool port_a_analog_trigger, uint8_t port_b_module, + uint32_t port_b_pin, bool port_b_analog_trigger, + bool reverseDirection, int32_t* index, + int32_t* status) { + // Initialize encoder structure + Encoder* encoder = new Encoder(); + + remapDigitalSource(port_a_analog_trigger, port_a_pin, port_a_module); + remapDigitalSource(port_b_analog_trigger, port_b_pin, port_b_module); + + hal::Resource::CreateResourceObject(&quadEncoders, tEncoder::kNumSystems); + encoder->index = quadEncoders->Allocate("4X Encoder"); + *index = encoder->index; + // TODO: if (index == ~0ul) { CloneError(quadEncoders); return; } + encoder->encoder = tEncoder::create(encoder->index, status); + encoder->encoder->writeConfig_ASource_Module(port_a_module, status); + encoder->encoder->writeConfig_ASource_Channel(port_a_pin, status); + encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger, + status); + encoder->encoder->writeConfig_BSource_Module(port_b_module, status); + encoder->encoder->writeConfig_BSource_Channel(port_b_pin, status); + encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger, + status); + encoder->encoder->strobeReset(status); + encoder->encoder->writeConfig_Reverse(reverseDirection, status); + encoder->encoder->writeTimerConfig_AverageSize(4, status); + + return encoder; +} + +void freeEncoder(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + if (!encoder) return; + quadEncoders->Free(encoder->index); + delete encoder->encoder; +} + +/** + * Reset the Encoder distance to zero. + * Resets the current count to zero on the encoder. + */ +void resetEncoder(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + encoder->encoder->strobeReset(status); +} + +/** + * Gets the raw value from the encoder. + * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale + * factor. + * @return Current raw count from the encoder + */ +int32_t getEncoder(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + return encoder->encoder->readOutput_Value(status); +} + +/** + * Returns the period of the most recent pulse. + * Returns the period of the most recent Encoder pulse in seconds. + * This method compenstates for the decoding type. + * + * @deprecated Use GetRate() in favor of this method. This returns unscaled + * periods and GetRate() scales using value from SetDistancePerPulse(). + * + * @return Period in seconds of the most recent pulse. + */ +double getEncoderPeriod(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status); + double value; + if (output.Stalled) { + // Return infinity + double zero = 0.0; + value = 1.0 / zero; + } else { + // output.Period is a fixed point number that counts by 2 (24 bits, 25 + // integer bits) + value = (double)(output.Period << 1) / (double)output.Count; + } + double measuredPeriod = value * 2.5e-8; + return measuredPeriod / DECODING_SCALING_FACTOR; +} + +/** + * Sets the maximum period for stopped detection. + * Sets the value that represents the maximum period of the Encoder before it + * will assume that the attached device is stopped. This timeout allows users + * to determine if the wheels or other shaft has stopped rotating. + * This method compensates for the decoding type. + * + * @deprecated Use SetMinRate() in favor of this method. This takes unscaled + * periods and SetMinRate() scales using value from SetDistancePerPulse(). + * + * @param maxPeriod The maximum time between rising and falling edges before the + * FPGA will + * report the device stopped. This is expressed in seconds. + */ +void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, + int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + encoder->encoder->writeTimerConfig_StallPeriod( + (uint32_t)(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR), status); +} + +/** + * Determine if the encoder is stopped. + * Using the MaxPeriod value, a boolean is returned that is true if the encoder + * is considered stopped and false if it is still moving. A stopped encoder is + * one where the most recent pulse width exceeds the MaxPeriod. + * @return True if the encoder is considered stopped. + */ +bool getEncoderStopped(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + return encoder->encoder->readTimerOutput_Stalled(status) != 0; +} + +/** + * The last direction the encoder value changed. + * @return The last direction the encoder value changed. + */ +bool getEncoderDirection(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + return encoder->encoder->readOutput_Direction(status); +} + +/** + * Set the direction sensing for this encoder. + * This sets the direction sensing on the encoder so that it could count in the + * correct software direction regardless of the mounting. + * @param reverseDirection true if the encoder direction should be reversed + */ +void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, + int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + encoder->encoder->writeConfig_Reverse(reverseDirection, status); +} + +/** + * Set the Samples to Average which specifies the number of samples of the timer + * to average when calculating the period. Perform averaging to account for + * mechanical imperfections or as oversampling to increase resolution. + * @param samplesToAverage The number of samples to average from 1 to 127. + */ +void setEncoderSamplesToAverage(void* encoder_pointer, + uint32_t samplesToAverage, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + if (samplesToAverage < 1 || samplesToAverage > 127) { + *status = PARAMETER_OUT_OF_RANGE; + } + encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status); +} + +/** + * Get the Samples to Average which specifies the number of samples of the timer + * to average when calculating the period. Perform averaging to account for + * mechanical imperfections or as oversampling to increase resolution. + * @return SamplesToAverage The number of samples being averaged (from 1 to 127) + */ +uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + return encoder->encoder->readTimerConfig_AverageSize(status); +} + +/** + * Set an index source for an encoder, which is an input that resets the + * encoder's count. + */ +void setEncoderIndexSource(void* encoder_pointer, uint32_t pin, + bool analogTrigger, bool activeHigh, + bool edgeSensitive, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status); + encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status); + encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger, + status); + encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status); + encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status); +} +} diff --git a/hal/lib/athena/I2C.cpp b/hal/lib/athena/I2C.cpp new file mode 100644 index 0000000000..b6383cd56f --- /dev/null +++ b/hal/lib/athena/I2C.cpp @@ -0,0 +1,167 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/I2C.h" + +#include "DigitalInternal.h" +#include "HAL/HAL.h" +#include "i2clib/i2c-lib.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +static priority_recursive_mutex digitalI2COnBoardMutex; +static priority_recursive_mutex digitalI2CMXPMutex; + +static uint8_t i2COnboardObjCount = 0; +static uint8_t i2CMXPObjCount = 0; +static uint8_t i2COnBoardHandle = 0; +static uint8_t i2CMXPHandle = 0; + +extern "C" { +/* + * Initialize the I2C port. Opens the port if necessary and saves the handle. + * If opening the MXP port, also sets up the pin functions appropriately + * @param port The port to open, 0 for the on-board, 1 for the MXP. + */ +void i2CInitialize(uint8_t port, int32_t* status) { + initializeDigital(status); + + if (port > 1) { + // Set port out of range error here + return; + } + + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + if (port == 0) { + i2COnboardObjCount++; + if (i2COnBoardHandle > 0) return; + i2COnBoardHandle = i2clib_open("/dev/i2c-2"); + } else if (port == 1) { + i2CMXPObjCount++; + if (i2CMXPHandle > 0) return; + if (!allocateDIO(getPort(24), false, status)) return; + if (!allocateDIO(getPort(25), false, status)) return; + digitalSystem->writeEnableMXPSpecialFunction( + digitalSystem->readEnableMXPSpecialFunction(status) | 0xC000, status); + i2CMXPHandle = i2clib_open("/dev/i2c-1"); + } + return; + } +} + +/** + * Generic transaction. + * + * This is a lower-level interface to the I2C hardware giving you more control + * over each transaction. + * + * @param dataToSend Buffer of data to send as part of the transaction. + * @param sendSize Number of bytes to send as part of the transaction. + * @param dataReceived Buffer to read data into. + * @param receiveSize Number of bytes to read from the device. + * @return The number of bytes read (>= 0) or -1 on transfer abort. + */ +int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, + uint8_t sendSize, uint8_t* dataReceived, + uint8_t receiveSize) { + if (port > 1) { + // Set port out of range error here + return -1; + } + + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + + { + std::lock_guard sync(lock); + return i2clib_writeread(handle, deviceAddress, (const char*)dataToSend, + (int32_t)sendSize, (char*)dataReceived, + (int32_t)receiveSize); + } +} + +/** + * Execute a write transaction with the device. + * + * Write a single byte to a register on a device and wait until the + * transaction is complete. + * + * @param registerAddress The address of the register on the device to be + * written. + * @param data The byte to write to the register on the device. + * @return The number of bytes written (>= 0) or -1 on transfer abort. + */ +int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, + uint8_t sendSize) { + if (port > 1) { + // Set port out of range error here + return -1; + } + + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + return i2clib_write(handle, deviceAddress, (const char*)dataToSend, + (int32_t)sendSize); + } +} + +/** + * Execute a read transaction with the device. + * + * Read bytes from a device. + * Most I2C devices will auto-increment the register pointer internally allowing + * you to read consecutive registers on a device in a single transaction. + * + * @param registerAddress The register to read first in the transaction. + * @param count The number of bytes to read in the transaction. + * @param buffer A pointer to the array of bytes to store the data read from the + * device. + * @return The number of bytes read (>= 0) or -1 on transfer abort. + */ +int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t* buffer, + uint8_t count) { + if (port > 1) { + // Set port out of range error here + return -1; + } + + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + return i2clib_read(handle, deviceAddress, (char*)buffer, (int32_t)count); + } +} + +void i2CClose(uint8_t port) { + if (port > 1) { + // Set port out of range error here + return; + } + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + if ((port == 0 ? i2COnboardObjCount-- : i2CMXPObjCount--) == 0) { + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + i2clib_close(handle); + } + } + return; +} +} diff --git a/hal/lib/athena/Interrupts.cpp b/hal/lib/athena/Interrupts.cpp index a8055c5ee0..5c33f4f6fe 100644 --- a/hal/lib/athena/Interrupts.cpp +++ b/hal/lib/athena/Interrupts.cpp @@ -9,8 +9,9 @@ #include "ChipObject.h" -extern void remapDigitalSource(bool analogTrigger, uint32_t& pin, - uint8_t& module); +#include "DigitalInternal.h" + +using namespace hal; struct Interrupt // FIXME: why is this internal? { diff --git a/hal/lib/athena/PWM.cpp b/hal/lib/athena/PWM.cpp new file mode 100644 index 0000000000..53b39aac9e --- /dev/null +++ b/hal/lib/athena/PWM.cpp @@ -0,0 +1,167 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/PWM.h" + +#include "DigitalInternal.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +extern "C" { +bool checkPWMChannel(void* digital_port_pointer) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + return port->port.pin < kPwmPins; +} + +/** + * Check a port to make sure that it is not nullptr and is a valid PWM port. + * + * Sets the status to contain the appropriate error. + * + * @return true if the port passed validation. + */ +static bool verifyPWMChannel(DigitalPort* port, int32_t* status) { + if (port == nullptr) { + *status = NULL_PARAMETER; + return false; + } else if (!checkPWMChannel(port)) { + *status = PARAMETER_OUT_OF_RANGE; + return false; + } else { + return true; + } +} + +/** + * Set a PWM channel to the desired value. The values range from 0 to 255 and + * the period is controlled + * by the PWM Period and MinHigh registers. + * + * @param channel The PWM channel to set. + * @param value The PWM value to set. + */ +void setPWM(void* digital_port_pointer, unsigned short value, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return; + } + + if (port->port.pin < tPWM::kNumHdrRegisters) { + pwmSystem->writeHdr(port->port.pin, value, status); + } else { + pwmSystem->writeMXP(port->port.pin - tPWM::kNumHdrRegisters, value, status); + } +} + +/** + * Get a value from a PWM channel. The values range from 0 to 255. + * + * @param channel The PWM channel to read from. + * @return The raw PWM value. + */ +unsigned short getPWM(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return 0; + } + + if (port->port.pin < tPWM::kNumHdrRegisters) { + return pwmSystem->readHdr(port->port.pin, status); + } else { + return pwmSystem->readMXP(port->port.pin - tPWM::kNumHdrRegisters, status); + } +} + +void latchPWMZero(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return; + } + + pwmSystem->writeZeroLatch(port->port.pin, true, status); + pwmSystem->writeZeroLatch(port->port.pin, false, status); +} + +/** + * Set how how often the PWM signal is squelched, thus scaling the period. + * + * @param channel The PWM channel to configure. + * @param squelchMask The 2-bit mask of outputs to squelch. + */ +void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, + int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return; + } + + if (port->port.pin < tPWM::kNumPeriodScaleHdrElements) { + pwmSystem->writePeriodScaleHdr(port->port.pin, squelchMask, status); + } else { + pwmSystem->writePeriodScaleMXP( + port->port.pin - tPWM::kNumPeriodScaleHdrElements, squelchMask, status); + } +} + +bool allocatePWMChannel(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return false; + } + + char buf[64]; + snprintf(buf, 64, "PWM %d", port->port.pin); + if (PWMChannels->Allocate(port->port.pin, buf) == ~0ul) { + *status = RESOURCE_IS_ALLOCATED; + return false; + } + + if (port->port.pin > tPWM::kNumHdrRegisters - 1) { + snprintf(buf, 64, "PWM %d and DIO %d", port->port.pin, + remapMXPPWMChannel(port->port.pin) + 10); + if (DIOChannels->Allocate(remapMXPPWMChannel(port->port.pin) + 10, buf) == + ~0ul) + return false; + uint32_t bitToSet = 1 << remapMXPPWMChannel(port->port.pin); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet, + status); + } + return true; +} + +void freePWMChannel(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!port) return; + if (!verifyPWMChannel(port, status)) { + return; + } + + PWMChannels->Free(port->port.pin); + if (port->port.pin > tPWM::kNumHdrRegisters - 1) { + DIOChannels->Free(remapMXPPWMChannel(port->port.pin) + 10); + uint32_t bitToUnset = 1 << remapMXPPWMChannel(port->port.pin); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset, + status); + } +} + +/** + * Get the loop timing of the PWM system + * + * @return The loop time + */ +uint16_t getLoopTiming(int32_t* status) { + return pwmSystem->readLoopTiming(status); +} +} diff --git a/hal/lib/athena/Relay.cpp b/hal/lib/athena/Relay.cpp new file mode 100644 index 0000000000..2ed8834e0c --- /dev/null +++ b/hal/lib/athena/Relay.cpp @@ -0,0 +1,118 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/Relay.h" + +#include "DigitalInternal.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +// Create a mutex to protect changes to the relay values +static priority_recursive_mutex digitalRelayMutex; + +constexpr uint32_t kRelayPins = 8; + +extern "C" { +bool checkRelayChannel(void* digital_port_pointer) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + return port->port.pin < kRelayPins; +} + +/** + * Check a port to make sure that it is not nullptr and is a valid Relay port. + * + * Sets the status to contain the appropriate error. + * + * @return true if the port passed validation. + */ +static bool verifyRelayChannel(DigitalPort* port, int32_t* status) { + if (port == nullptr) { + *status = NULL_PARAMETER; + return false; + } else if (!checkRelayChannel(port)) { + *status = PARAMETER_OUT_OF_RANGE; + return false; + } else { + return true; + } +} + +/** + * Set the state of a relay. + * Set the state of a relay output to be forward. Relays have two outputs and + * each is + * independently set to 0v or 12v. + */ +void setRelayForward(void* digital_port_pointer, bool on, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyRelayChannel(port, status)) { + return; + } + + { + std::lock_guard sync(digitalRelayMutex); + uint8_t forwardRelays = relaySystem->readValue_Forward(status); + if (on) + forwardRelays |= 1 << port->port.pin; + else + forwardRelays &= ~(1 << port->port.pin); + relaySystem->writeValue_Forward(forwardRelays, status); + } +} + +/** + * Set the state of a relay. + * Set the state of a relay output to be reverse. Relays have two outputs and + * each is + * independently set to 0v or 12v. + */ +void setRelayReverse(void* digital_port_pointer, bool on, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyRelayChannel(port, status)) { + return; + } + + { + std::lock_guard sync(digitalRelayMutex); + uint8_t reverseRelays = relaySystem->readValue_Reverse(status); + if (on) + reverseRelays |= 1 << port->port.pin; + else + reverseRelays &= ~(1 << port->port.pin); + relaySystem->writeValue_Reverse(reverseRelays, status); + } +} + +/** + * Get the current state of the forward relay channel + */ +bool getRelayForward(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyRelayChannel(port, status)) { + return false; + } + + uint8_t forwardRelays = relaySystem->readValue_Forward(status); + return (forwardRelays & (1 << port->port.pin)) != 0; +} + +/** + * Get the current state of the reverse relay channel + */ +bool getRelayReverse(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyRelayChannel(port, status)) { + return false; + } + + uint8_t reverseRelays = relaySystem->readValue_Reverse(status); + return (reverseRelays & (1 << port->port.pin)) != 0; +} +} diff --git a/hal/lib/athena/SPI.cpp b/hal/lib/athena/SPI.cpp new file mode 100644 index 0000000000..be90346350 --- /dev/null +++ b/hal/lib/athena/SPI.cpp @@ -0,0 +1,550 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/SPI.h" + +#include "DigitalInternal.h" +#include "HAL/HAL.h" +#include "spilib/spi-lib.h" + +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); + +using namespace hal; + +static int32_t m_spiCS0Handle = 0; +static int32_t m_spiCS1Handle = 0; +static int32_t m_spiCS2Handle = 0; +static int32_t m_spiCS3Handle = 0; +static int32_t m_spiMXPHandle = 0; +static priority_recursive_mutex spiOnboardSemaphore; +static priority_recursive_mutex spiMXPSemaphore; +static tSPI* spiSystem; + +extern "C" { + +struct SPIAccumulator { + void* notifier = nullptr; + uint64_t triggerTime; + uint32_t period; + + int64_t value = 0; + uint32_t count = 0; + int32_t last_value = 0; + + int32_t center = 0; + int32_t deadband = 0; + + uint8_t cmd[4]; // command to send (up to 4 bytes) + uint32_t valid_mask; + uint32_t valid_value; + int32_t data_max; // one more than max data value + int32_t data_msb_mask; // data field MSB mask (for signed) + uint8_t data_shift; // data field shift right amount, in bits + uint8_t xfer_size; // SPI transfer size, in bytes (up to 4) + uint8_t port; + bool is_signed; // is data field signed? + bool big_endian; // is response big endian? +}; +SPIAccumulator* spiAccumulators[5] = {nullptr, nullptr, nullptr, nullptr, + nullptr}; + +/* + * Initialize the spi port. Opens the port if necessary and saves the handle. + * If opening the MXP port, also sets up the pin functions appropriately + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + */ +void spiInitialize(uint8_t port, int32_t* status) { + if (spiSystem == nullptr) spiSystem = tSPI::create(status); + if (spiGetHandle(port) != 0) return; + switch (port) { + case 0: + spiSetHandle(0, spilib_open("/dev/spidev0.0")); + break; + case 1: + spiSetHandle(1, spilib_open("/dev/spidev0.1")); + break; + case 2: + spiSetHandle(2, spilib_open("/dev/spidev0.2")); + break; + case 3: + spiSetHandle(3, spilib_open("/dev/spidev0.3")); + break; + case 4: + initializeDigital(status); + if (!allocateDIO(getPort(14), false, status)) { + printf("Failed to allocate DIO 14\n"); + return; + } + if (!allocateDIO(getPort(15), false, status)) { + printf("Failed to allocate DIO 15\n"); + return; + } + if (!allocateDIO(getPort(16), true, status)) { + printf("Failed to allocate DIO 16\n"); + return; + } + if (!allocateDIO(getPort(17), false, status)) { + printf("Failed to allocate DIO 17\n"); + return; + } + digitalSystem->writeEnableMXPSpecialFunction( + digitalSystem->readEnableMXPSpecialFunction(status) | 0x00F0, status); + spiSetHandle(4, spilib_open("/dev/spidev1.0")); + break; + default: + break; + } + return; +} + +/** + * Generic transaction. + * + * This is a lower-level interface to the spi hardware giving you more control + * over each transaction. + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @param dataToSend Buffer of data to send as part of the transaction. + * @param dataReceived Buffer to read data into. + * @param size Number of bytes to transfer. [0..7] + * @return Number of bytes transferred, -1 for error + */ +int32_t spiTransaction(uint8_t port, uint8_t* dataToSend, uint8_t* dataReceived, + uint8_t size) { + std::lock_guard sync(spiGetSemaphore(port)); + return spilib_writeread(spiGetHandle(port), (const char*)dataToSend, + (char*)dataReceived, (int32_t)size); +} + +/** + * Execute a write transaction with the device. + * + * Write to a device and wait until the transaction is complete. + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @param datToSend The data to write to the register on the device. + * @param sendSize The number of bytes to be written + * @return The number of bytes written. -1 for an error + */ +int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize) { + std::lock_guard sync(spiGetSemaphore(port)); + return spilib_write(spiGetHandle(port), (const char*)dataToSend, + (int32_t)sendSize); +} + +/** + * Execute a read from the device. + * + * This method does not write any data out to the device + * Most spi devices will require a register address to be written before + * they begin returning data + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @param buffer A pointer to the array of bytes to store the data read from the + * device. + * @param count The number of bytes to read in the transaction. [1..7] + * @return Number of bytes read. -1 for error. + */ +int32_t spiRead(uint8_t port, uint8_t* buffer, uint8_t count) { + std::lock_guard sync(spiGetSemaphore(port)); + return spilib_read(spiGetHandle(port), (char*)buffer, (int32_t)count); +} + +/** + * Close the SPI port + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + */ +void spiClose(uint8_t port) { + std::lock_guard sync(spiGetSemaphore(port)); + if (spiAccumulators[port]) { + int32_t status = 0; + spiFreeAccumulator(port, &status); + } + spilib_close(spiGetHandle(port)); + spiSetHandle(port, 0); + return; +} + +/** + * Set the clock speed for the SPI bus. + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @param speed The speed in Hz (0-1MHz) + */ +void spiSetSpeed(uint8_t port, uint32_t speed) { + std::lock_guard sync(spiGetSemaphore(port)); + spilib_setspeed(spiGetHandle(port), speed); +} + +/** + * Set the SPI options + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @param msb_first True to write the MSB first, False for LSB first + * @param sample_on_trailing True to sample on the trailing edge, False to + * sample on the leading edge + * @param clk_idle_high True to set the clock to active low, False to set the + * clock active high + */ +void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, + int clk_idle_high) { + std::lock_guard sync(spiGetSemaphore(port)); + spilib_setopts(spiGetHandle(port), msb_first, sample_on_trailing, + clk_idle_high); +} + +/** + * Set the CS Active high for a SPI port + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + */ +void spiSetChipSelectActiveHigh(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port < 4) { + spiSystem->writeChipSelectActiveHigh_Hdr( + spiSystem->readChipSelectActiveHigh_Hdr(status) | (1 << port), status); + } else { + spiSystem->writeChipSelectActiveHigh_MXP(1, status); + } +} + +/** + * Set the CS Active low for a SPI port + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + */ +void spiSetChipSelectActiveLow(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port < 4) { + spiSystem->writeChipSelectActiveHigh_Hdr( + spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1 << port), status); + } else { + spiSystem->writeChipSelectActiveHigh_MXP(0, status); + } +} + +/** + * Get the stored handle for a SPI port + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @return The stored handle for the SPI port. 0 represents no stored handle. + */ +int32_t spiGetHandle(uint8_t port) { + std::lock_guard sync(spiGetSemaphore(port)); + switch (port) { + case 0: + return m_spiCS0Handle; + case 1: + return m_spiCS1Handle; + case 2: + return m_spiCS2Handle; + case 3: + return m_spiCS3Handle; + case 4: + return m_spiMXPHandle; + default: + return 0; + } +} + +/** + * Set the stored handle for a SPI port + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for + * MXP. + * @param handle The value of the handle for the port. + */ +void spiSetHandle(uint8_t port, int32_t handle) { + std::lock_guard sync(spiGetSemaphore(port)); + switch (port) { + case 0: + m_spiCS0Handle = handle; + break; + case 1: + m_spiCS1Handle = handle; + break; + case 2: + m_spiCS2Handle = handle; + break; + case 3: + m_spiCS3Handle = handle; + break; + case 4: + m_spiMXPHandle = handle; + break; + default: + break; + } +} + +/** + * Get the semaphore for a SPI port + * + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP + * @return The semaphore for the SPI port. + */ +extern "C++" priority_recursive_mutex& spiGetSemaphore(uint8_t port) { + if (port < 4) + return spiOnboardSemaphore; + else + return spiMXPSemaphore; +} + +static void spiAccumulatorProcess(uint64_t currentTime, void* param) { + SPIAccumulator* accum = (SPIAccumulator*)param; + + // perform SPI transaction + uint8_t resp_b[4]; + std::lock_guard sync(spiGetSemaphore(accum->port)); + spilib_writeread(spiGetHandle(accum->port), (const char*)accum->cmd, + (char*)resp_b, (int32_t)accum->xfer_size); + + // convert from bytes + uint32_t resp = 0; + if (accum->big_endian) { + for (int i = 0; i < accum->xfer_size; ++i) { + resp <<= 8; + resp |= resp_b[i] & 0xff; + } + } else { + for (int i = accum->xfer_size - 1; i >= 0; --i) { + resp <<= 8; + resp |= resp_b[i] & 0xff; + } + } + + // process response + if ((resp & accum->valid_mask) == accum->valid_value) { + // valid sensor data; extract data field + int32_t data = (int32_t)(resp >> accum->data_shift); + data &= accum->data_max - 1; + // 2s complement conversion if signed MSB is set + if (accum->is_signed && (data & accum->data_msb_mask) != 0) + data -= accum->data_max; + // center offset + data -= accum->center; + // only accumulate if outside deadband + if (data < -accum->deadband || data > accum->deadband) accum->value += data; + ++accum->count; + accum->last_value = data; + } else { + // no data from the sensor; just clear the last value + accum->last_value = 0; + } + + // reschedule timer + accum->triggerTime += accum->period; + // handle timer slip + if (accum->triggerTime < currentTime) + accum->triggerTime = currentTime + accum->period; + int32_t status = 0; + updateNotifierAlarm(accum->notifier, accum->triggerTime, &status); +} + +/** + * Initialize a SPI accumulator. + * + * @param port SPI port + * @param period Time between reads, in us + * @param cmd SPI command to send to request data + * @param xfer_size SPI transfer size, in bytes + * @param valid_mask Mask to apply to received data for validity checking + * @param valid_data After valid_mask is applied, required matching value for + * validity checking + * @param data_shift Bit shift to apply to received data to get actual data + * value + * @param data_size Size (in bits) of data field + * @param is_signed Is data field signed? + * @param big_endian Is device big endian? + */ +void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, + uint8_t xfer_size, uint32_t valid_mask, + uint32_t valid_value, uint8_t data_shift, + uint8_t data_size, bool is_signed, bool big_endian, + int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port > 4) return; + if (!spiAccumulators[port]) spiAccumulators[port] = new SPIAccumulator(); + SPIAccumulator* accum = spiAccumulators[port]; + if (big_endian) { + for (int i = xfer_size - 1; i >= 0; --i) { + accum->cmd[i] = cmd & 0xff; + cmd >>= 8; + } + } else { + accum->cmd[0] = cmd & 0xff; + cmd >>= 8; + accum->cmd[1] = cmd & 0xff; + cmd >>= 8; + accum->cmd[2] = cmd & 0xff; + cmd >>= 8; + accum->cmd[3] = cmd & 0xff; + } + accum->period = period; + accum->xfer_size = xfer_size; + accum->valid_mask = valid_mask; + accum->valid_value = valid_value; + accum->data_shift = data_shift; + accum->data_max = (1 << data_size); + accum->data_msb_mask = (1 << (data_size - 1)); + accum->is_signed = is_signed; + accum->big_endian = big_endian; + if (!accum->notifier) { + accum->notifier = initializeNotifier(spiAccumulatorProcess, accum, status); + accum->triggerTime = getFPGATime(status) + period; + if (*status != 0) return; + updateNotifierAlarm(accum->notifier, accum->triggerTime, status); + } +} + +/** + * Frees a SPI accumulator. + */ +void spiFreeAccumulator(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + cleanNotifier(accum->notifier, status); + delete accum; + spiAccumulators[port] = nullptr; +} + +/** + * Resets the accumulator to zero. + */ +void spiResetAccumulator(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->value = 0; + accum->count = 0; + accum->last_value = 0; +} + +/** + * Set the center value of the accumulator. + * + * The center value is subtracted from each value before it is added to the + * accumulator. This + * is used for the center value of devices like gyros and accelerometers to make + * integration work + * and to take the device offset into account when integrating. + */ +void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->center = center; +} + +/** + * Set the accumulator's deadband. + */ +void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, + int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->deadband = deadband; +} + +/** + * Read the last value read by the accumulator engine. + */ +int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->last_value; +} + +/** + * Read the accumulated value. + * + * @return The 64-bit value accumulated since the last Reset(). + */ +int64_t spiGetAccumulatorValue(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->value; +} + +/** + * Read the number of accumulated values. + * + * Read the count of the accumulated values since the accumulator was last + * Reset(). + * + * @return The number of times samples from the channel were accumulated. + */ +uint32_t spiGetAccumulatorCount(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->count; +} + +/** + * Read the average of the accumulated value. + * + * @return The accumulated average value (value / count). + */ +double spiGetAccumulatorAverage(uint8_t port, int32_t* status) { + int64_t value; + uint32_t count; + spiGetAccumulatorOutput(port, &value, &count, status); + if (count == 0) return 0.0; + return ((double)value) / count; +} + +/** + * Read the accumulated value and the number of accumulated values atomically. + * + * This function reads the value and count atomically. + * This can be used for averaging. + * + * @param value Pointer to the 64-bit accumulated output. + * @param count Pointer to the number of accumulation cycles. + */ +void spiGetAccumulatorOutput(uint8_t port, int64_t* value, uint32_t* count, + int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + *value = 0; + *count = 0; + return; + } + *value = accum->value; + *count = accum->count; +} +} diff --git a/wpilibj/src/athena/cpp/lib/SPIJNI.cpp b/wpilibj/src/athena/cpp/lib/SPIJNI.cpp index e252eb6214..0d9485c09e 100644 --- a/wpilibj/src/athena/cpp/lib/SPIJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/SPIJNI.cpp @@ -12,7 +12,6 @@ #include "edu_wpi_first_wpilibj_hal_SPIJNI.h" #include "HAL/SPI.h" -#include "HAL/SPIAccumulator.h" #include "HALUtil.h" // set the logging level