/**************************************************************************************************** RepRapFirmware - Platform: RepRapPro Ormerod with Arduino Due controller Platform contains all the code and definitions to deal with machine-dependent things such as control pins, bed area, number of extruders, tolerable accelerations and speeds and so on. ----------------------------------------------------------------------------------------------------- Version 0.1 18 November 2012 Adrian Bowyer RepRap Professional Ltd http://reprappro.com Licence: GPL ****************************************************************************************************/ #include "RepRapFirmware.h" #include "DueFlashStorage.h" extern char _end; extern "C" char *sbrk(int i); const uint8_t memPattern = 0xA5; const int Dac0DigitalPin = 66; // Arduino Due pin number corresponding to DAC0 output pin static uint32_t fanInterruptCount = 0; // accessed only in ISR, so no need to declare it volatile const uint32_t fanMaxInterruptCount = 32; // number of fan interrupts that we average over static volatile uint32_t fanLastResetTime = 0; // time (microseconds) at which we last reset the interrupt count, accessed inside and outside ISR static volatile uint32_t fanInterval = 0; // written by ISR, read outside the ISR //#define MOVE_DEBUG #ifdef MOVE_DEBUG unsigned int numInterruptsScheduled = 0; unsigned int numInterruptsExecuted = 0; uint32_t nextInterruptTime = 0; uint32_t nextInterruptScheduledAt = 0; uint32_t lastInterruptTime = 0; #endif // Arduino initialise and loop functions // Put nothing in these other than calls to the RepRap equivalents void setup() { // Fill the free memory with a pattern so that we can check for stack usage and memory corruption char* heapend = sbrk(0); register const char * stack_ptr asm ("sp"); while (heapend + 16 < stack_ptr) { *heapend++ = memPattern; } reprap.Init(); } void loop() { reprap.Spin(); } extern "C" { // This intercepts the 1ms system tick. It must return 'false', otherwise the Arduino core tick handler will be bypassed. int sysTickHook() { reprap.Tick(); return 0; } } //************************************************************************************************* // PidParameters class bool PidParameters::UsePID() const { return kP >= 0; } float PidParameters::GetThermistorR25() const { return thermistorInfR * exp(thermistorBeta / (25.0 - ABS_ZERO)); } void PidParameters::SetThermistorR25AndBeta(float r25, float beta) { thermistorInfR = r25 * exp(-beta / (25.0 - ABS_ZERO)); thermistorBeta = beta; } bool PidParameters::operator==(const PidParameters& other) const { return kI == other.kI && kD == other.kD && kP == other.kP && kT == other.kT && kS == other.kS && fullBand == other.fullBand && pidMin == other.pidMin && pidMax == other.pidMax && thermistorBeta == other.thermistorBeta && thermistorInfR == other.thermistorInfR && thermistorSeriesR == other.thermistorSeriesR && adcLowOffset == other.adcLowOffset && adcHighOffset == other.adcHighOffset; } //************************************************************************************************* // Platform class // Definition of which pins we allow to be controlled using M42 // The allowed pins are these ones on the DueX4 expansion connector: // TXD1 aka PA13 aka pin 16 // RXD1 aka PA12 aka pin 17 // TXD0 aka PA11 aka pin 18 // RXD0 aka PA10 aka pin 19 // PC4_PWML1 aka PC4 aka pin 36 // AD13 aka PB20 aka pin 66 // AD14 aka PB21 aka pin 52 // PB16 aka pin 67 (could possibly allow analog output on this one) // RTS1 aka PA14 aka pin 23 // TWD1 aka PB12 aka pin 20 // TWCK1 aka PB13 aka pin 21 /*static*/ const uint8_t Platform::pinAccessAllowed[numPins/8] = { 0, // pins 0-7 0, // pins 8-15 0b10111111, // pins 16-23 0, // pins 24-31 0b00010000, // pins 32-39 0, // pins 40-47 0b00010000, // pins 48-55 0, // pins 46-63 0b00001100 // pins 64-71 }; Platform::Platform() : autoSaveEnabled(false), board(BoardType::Duet_06), active(false), errorCodeBits(0), auxOutputBuffer(nullptr), aux2OutputBuffer(nullptr), usbOutputBuffer(nullptr), fileStructureInitialised(false), tickState(0), debugCode(0), messageString(messageStringBuffer, ARRAY_SIZE(messageStringBuffer)) { // Files massStorage = new MassStorage(this); for (size_t i = 0; i < MAX_FILES; i++) { files[i] = new FileStore(this); } } //******************************************************************************************************************* void Platform::Init() { // Deal with power first digitalWriteNonDue(atxPowerPin, LOW); // ensure ATX power is off by default pinModeNonDue(atxPowerPin, OUTPUT); SetBoardType(BoardType::Auto); // Comms baudRates[0] = USB_BAUD_RATE; baudRates[1] = AUX_BAUD_RATE; baudRates[2] = AUX2_BAUD_RATE; commsParams[0] = 0; commsParams[1] = 1; // by default we require a checksum on data from the aux port, to guard against overrun errors SerialUSB.begin(baudRates[0]); Serial.begin(baudRates[1]); // this can't be done in the constructor because the Arduino port initialisation isn't complete at that point Serial1.begin(baudRates[2]); static_assert(sizeof(FlashData) + sizeof(SoftwareResetData) <= FLASH_DATA_LENGTH, "NVData too large"); ResetNvData(); // We need to initialise at least some of the time stuff before we call MassStorage::Init() addToTime = 0.0; lastTimeCall = 0; lastTime = Time(); longWait = lastTime; // File management massStorage->Init(); for (size_t file = 0; file < MAX_FILES; file++) { files[file]->Init(); } fileStructureInitialised = true; mcpDuet.begin(); // only call begin once in the entire execution, this begins the I2C comms on that channel for all objects mcpExpansion.setMCP4461Address(0x2E); // not required for mcpDuet, as this uses the default address // Directories sysDir = SYS_DIR; macroDir = MACRO_DIR; webDir = WEB_DIR; gcodeDir = GCODE_DIR; configFile = CONFIG_FILE; defaultFile = DEFAULT_FILE; // DRIVES ARRAY_INIT(stepPins, STEP_PINS); ARRAY_INIT(directionPins, DIRECTION_PINS); ARRAY_INIT(directions, DIRECTIONS); ARRAY_INIT(enableValues, ENABLE_VALUES); ARRAY_INIT(enablePins, ENABLE_PINS); ARRAY_INIT(endStopPins, END_STOP_PINS); ARRAY_INIT(maxFeedrates, MAX_FEEDRATES); ARRAY_INIT(accelerations, ACCELERATIONS); ARRAY_INIT(driveStepsPerUnit, DRIVE_STEPS_PER_UNIT); ARRAY_INIT(instantDvs, INSTANT_DVS); ARRAY_INIT(potWipes, POT_WIPES); senseResistor = SENSE_RESISTOR; maxStepperDigipotVoltage = MAX_STEPPER_DIGIPOT_VOLTAGE; maxStepperDACVoltage = MAX_STEPPER_DAC_VOLTAGE; // Z PROBE zProbePin = Z_PROBE_PIN; zProbeAdcChannel = PinToAdcChannel(zProbePin); InitZProbe(); // this also sets up zProbeModulationPin // AXES ARRAY_INIT(axisMaxima, AXIS_MAXIMA); ARRAY_INIT(axisMinima, AXIS_MINIMA); idleCurrentFactor = DEFAULT_IDLE_CURRENT_FACTOR; SetSlowestDrive(); // HEATERS - Bed is assumed to be the first ARRAY_INIT(tempSensePins, TEMP_SENSE_PINS); ARRAY_INIT(heatOnPins, HEAT_ON_PINS); ARRAY_INIT(standbyTemperatures, STANDBY_TEMPERATURES); ARRAY_INIT(activeTemperatures, ACTIVE_TEMPERATURES); heatSampleTime = HEAT_SAMPLE_TIME; timeToHot = TIME_TO_HOT; // Motors for (size_t drive = 0; drive < DRIVES; drive++) { SetPhysicalDrive(drive, drive); // map drivers directly to axes and extruders if (stepPins[drive] >= 0) { pinModeNonDue(stepPins[drive], OUTPUT); } if (directionPins[drive] >= 0) { pinModeNonDue(directionPins[drive], OUTPUT); } if (enablePins[drive] >= 0) { pinModeNonDue(enablePins[drive], OUTPUT); } if (endStopPins[drive] >= 0) { pinModeNonDue(endStopPins[drive], INPUT_PULLUP); } motorCurrents[drive] = 0.0; DisableDrive(drive); driveState[drive] = DriveStatus::disabled; SetElasticComp(drive, 0.0); if (drive <= AXES) { endStopType[drive] = (drive == Y_AXIS) ? EndStopType::lowEndStop // for Ormerod 2/Huxley/Mendel compatibility : EndStopType::noEndStop; // for Ormerod/Huxley/Mendel compatibility endStopLogicLevel[drive] = true; // assume all endstops use active high logic e.g. normally-closed switch to ground } } extrusionAncilliaryPWM = 0.0; // HEATERS - Bed is assumed to be index 0 for (size_t heater = 0; heater < HEATERS; heater++) { if (heatOnPins[heater] >= 0) { digitalWriteNonDue(heatOnPins[heater], HIGH); // turn the heater off pinModeNonDue(heatOnPins[heater], OUTPUT); } analogReadResolution(12); SetThermistorNumber(heater, heater); // map the thermistor straight through thermistorFilters[heater].Init(analogRead(tempSensePins[heater])); // Calculate and store the ADC average sum that corresponds to an overheat condition, so that we can check is quickly in the tick ISR float thermistorOverheatResistance = nvData.pidParams[heater].GetRInf() * exp(-nvData.pidParams[heater].GetBeta() / (BAD_HIGH_TEMPERATURE - ABS_ZERO)); float thermistorOverheatAdcValue = (AD_RANGE_REAL + 1) * thermistorOverheatResistance / (thermistorOverheatResistance + nvData.pidParams[heater].thermistorSeriesR); thermistorOverheatSums[heater] = (uint32_t) (thermistorOverheatAdcValue + 0.9) * THERMISTOR_AVERAGE_READINGS; } InitFans(); // Hotend configuration nozzleDiameter = NOZZLE_DIAMETER; filamentWidth = FILAMENT_WIDTH; // Clear the spare pin configuration memset(pinInitialised, 0, sizeof(pinInitialised)); // Kick everything off lastTime = Time(); longWait = lastTime; InitialiseInterrupts(); // also sets 'active' to true } // Specify which thermistor channel a particular heater uses void Platform::SetThermistorNumber(size_t heater, size_t thermistor) //pre(heater < HEATERS && thermistor < HEATERS) { heaterAdcChannels[heater] = PinToAdcChannel(tempSensePins[thermistor]); } int Platform::GetThermistorNumber(size_t heater) const { for (size_t thermistor = 0; thermistor < HEATERS; ++thermistor) { if (heaterAdcChannels[heater] == PinToAdcChannel(tempSensePins[thermistor])) { return thermistor; } } return -1; } void Platform::SetSlowestDrive() { slowestDrive = 0; for (size_t drive = 1; drive < DRIVES; drive++) { if (ConfiguredInstantDv(drive) < ConfiguredInstantDv(slowestDrive)) { slowestDrive = drive; } } } void Platform::InitZProbe() { zProbeOnFilter.Init(0); zProbeOffFilter.Init(0); zProbeModulationPin = (board == BoardType::Duet_07 || board == BoardType::Duet_085) ? Z_PROBE_MOD_PIN07 : Z_PROBE_MOD_PIN; switch (nvData.zProbeType) { case 1: case 2: pinModeNonDue(zProbeModulationPin, OUTPUT); digitalWriteNonDue(zProbeModulationPin, HIGH); // enable the IR LED break; case 3: pinModeNonDue(zProbeModulationPin, OUTPUT); digitalWriteNonDue(zProbeModulationPin, LOW); // enable the alternate sensor break; case 4: pinModeNonDue(endStopPins[E0_AXIS], INPUT_PULLUP); break; case 5: break; //TODO default: break; } } // Return the Z probe data. // The ADC readings are 12 bits, so we convert them to 10-bit readings for compatibility with the old firmware. int Platform::ZProbe() const { if (zProbeOnFilter.IsValid() && zProbeOffFilter.IsValid()) { switch (nvData.zProbeType) { case 1: // Simple or intelligent IR sensor case 3: // Alternate sensor case 4: // Mechanical Z probe return (int) ((zProbeOnFilter.GetSum() + zProbeOffFilter.GetSum()) / (8 * Z_PROBE_AVERAGE_READINGS)); case 2: // Modulated IR sensor. // We assume that zProbeOnFilter and zProbeOffFilter average the same number of readings. // Because of noise, it is possible to get a negative reading, so allow for this. return (int) (((int32_t) zProbeOnFilter.GetSum() - (int32_t) zProbeOffFilter.GetSum()) / (int)(4 * Z_PROBE_AVERAGE_READINGS)); case 5: return (int) ((zProbeOnFilter.GetSum() + zProbeOffFilter.GetSum()) / (8 * Z_PROBE_AVERAGE_READINGS)); //TODO this is temporary default: break; } } return 0; // Z probe not turned on or not initialised yet } // Return the Z probe secondary values. int Platform::GetZProbeSecondaryValues(int& v1, int& v2) { if (zProbeOnFilter.IsValid() && zProbeOffFilter.IsValid()) { switch (nvData.zProbeType) { case 2: // modulated IR sensor v1 = (int) (zProbeOnFilter.GetSum() / (4 * Z_PROBE_AVERAGE_READINGS)); // pass back the reading with IR turned on return 1; default: break; } } return 0; } int Platform::GetZProbeType() const { return nvData.zProbeType; } void Platform::SetZProbeAxes(const bool axes[AXES]) { for (size_t axis=0; axis= 0 && pt <= 5) ? pt : 0; if (newZProbeType != nvData.zProbeType) { nvData.zProbeType = newZProbeType; if (autoSaveEnabled) { WriteNvData(); } } InitZProbe(); } const ZProbeParameters& Platform::GetZProbeParameters() const { switch (nvData.zProbeType) { case 0: case 4: default: return nvData.switchZProbeParameters; case 1: case 2: return nvData.irZProbeParameters; case 3: case 5: return nvData.alternateZProbeParameters; } } bool Platform::SetZProbeParameters(const struct ZProbeParameters& params) { switch (nvData.zProbeType) { case 0: case 4: if (nvData.switchZProbeParameters != params) { nvData.switchZProbeParameters = params; if (autoSaveEnabled) { WriteNvData(); } } return true; case 1: case 2: if (nvData.irZProbeParameters != params) { nvData.irZProbeParameters = params; if (autoSaveEnabled) { WriteNvData(); } } return true; case 3: case 5: if (nvData.alternateZProbeParameters != params) { nvData.alternateZProbeParameters = params; if (autoSaveEnabled) { WriteNvData(); } } return true; default: return false; } } // Return true if we must home X and Y before we home Z (i.e. we are using a bed probe) bool Platform::MustHomeXYBeforeZ() const { return nvData.zProbeType != 0 && nvData.zProbeAxes[Z_AXIS]; } void Platform::ResetNvData() { nvData.compatibility = marlin; // default to Marlin because the common host programs expect the "OK" response to commands ARRAY_INIT(nvData.ipAddress, IP_ADDRESS); ARRAY_INIT(nvData.netMask, NET_MASK); ARRAY_INIT(nvData.gateWay, GATE_WAY); ARRAY_INIT(nvData.macAddress, MAC_ADDRESS); nvData.zProbeType = 0; // Default is to use no Z probe switch ARRAY_INIT(nvData.zProbeAxes, Z_PROBE_AXES); nvData.switchZProbeParameters.Init(0.0); nvData.irZProbeParameters.Init(Z_PROBE_STOP_HEIGHT); nvData.alternateZProbeParameters.Init(Z_PROBE_STOP_HEIGHT); for (size_t i = 0; i < HEATERS; ++i) { PidParameters& pp = nvData.pidParams[i]; pp.thermistorSeriesR = defaultThermistorSeriesRs[i]; pp.SetThermistorR25AndBeta(defaultThermistor25RS[i], defaultThermistorBetas[i]); pp.kI = defaultPidKis[i]; pp.kD = defaultPidKds[i]; pp.kP = defaultPidKps[i]; pp.kT = defaultPidKts[i]; pp.kS = defaultPidKss[i]; pp.fullBand = defaultFullBands[i]; pp.pidMin = defaultPidMins[i]; pp.pidMax = defaultPidMaxes[i]; pp.adcLowOffset = pp.adcHighOffset = 0.0; } #if FLASH_SAVE_ENABLED nvData.magic = FlashData::magicValue; #endif } void Platform::ReadNvData() { #if FLASH_SAVE_ENABLED DueFlashStorage::read(FlashData::nvAddress, &nvData, sizeof(nvData)); if (nvData.magic != FlashData::magicValue) { // Nonvolatile data has not been initialized since the firmware was last written, so set up default values ResetNvData(); // No point in writing it back here } #else Message(BOTH_ERROR_MESSAGE, "Cannot load non-volatile data, because Flash support has been disabled!\n"); #endif } void Platform::WriteNvData() { #if FLASH_SAVE_ENABLED DueFlashStorage::write(FlashData::nvAddress, &nvData, sizeof(nvData)); #else Message(BOTH_ERROR_MESSAGE, "Cannot write non-volatile data, because Flash support has been disabled!\n"); #endif } void Platform::SetAutoSave(bool enabled) { #if FLASH_SAVE_ENABLED autoSaveEnabled = enabled; #else Message(BOTH_ERROR_MESSAGE, "Cannot enable auto-save, because Flash support has been disabled!\n"); #endif } // Send the beep command to the aux channel. There is no flow control on this port, so it can't block for long. void Platform::Beep(int freq, int ms) { MessageF(AUX_MESSAGE, "{\"beep_freq\":%d,\"beep_length\":%d}\n", freq, ms); } // Note: the use of floating point time will cause the resolution to degrade over time. // For example, 1ms time resolution will only be available for about half an hour from startup. // Personally, I (dc42) would rather just maintain and provide the time in milliseconds in a uint32_t. // This would wrap round after about 49 days, but that isn't difficult to handle. float Platform::Time() { unsigned long now = micros(); if (now < lastTimeCall) // Has timer overflowed? { addToTime += ((float) ULONG_MAX) * TIME_FROM_REPRAP; } lastTimeCall = now; return addToTime + TIME_FROM_REPRAP * (float) now; } void Platform::Exit() { Message(GENERIC_MESSAGE, "Platform class exited.\n"); active = false; } Compatibility Platform::Emulating() const { if (nvData.compatibility == reprapFirmware) return me; return nvData.compatibility; } void Platform::SetEmulating(Compatibility c) { if (c != me && c != reprapFirmware && c != marlin) { Message(GENERIC_MESSAGE, "Attempt to emulate unsupported firmware.\n"); return; } if (c == reprapFirmware) { c = me; } if (c != nvData.compatibility) { nvData.compatibility = c; if (autoSaveEnabled) { WriteNvData(); } } } void Platform::UpdateNetworkAddress(byte dst[4], const byte src[4]) { bool changed = false; for (uint8_t i = 0; i < 4; i++) { if (dst[i] != src[i]) { dst[i] = src[i]; changed = true; } } if (changed && autoSaveEnabled) { WriteNvData(); } } void Platform::SetIPAddress(byte ip[]) { UpdateNetworkAddress(nvData.ipAddress, ip); } void Platform::SetGateWay(byte gw[]) { UpdateNetworkAddress(nvData.gateWay, gw); } void Platform::SetNetMask(byte nm[]) { UpdateNetworkAddress(nvData.netMask, nm); } void Platform::Spin() { if (!active) return; // Write non-blocking data to the AUX line if (auxOutputBuffer != nullptr) { size_t bytesToWrite = min(Serial.canWrite(), auxOutputBuffer->BytesLeft()); if (bytesToWrite > 0) { Serial.write(auxOutputBuffer->Read(bytesToWrite), bytesToWrite); } if (auxOutputBuffer->BytesLeft() == 0) { auxOutputBuffer = reprap.ReleaseOutput(auxOutputBuffer); } } // Write non-blocking data to the second AUX line if (aux2OutputBuffer != nullptr) { size_t bytesToWrite = min(Serial1.canWrite(), aux2OutputBuffer->BytesLeft()); if (bytesToWrite > 0) { Serial1.write(aux2OutputBuffer->Read(bytesToWrite), bytesToWrite); } if (aux2OutputBuffer->BytesLeft() == 0) { aux2OutputBuffer = reprap.ReleaseOutput(aux2OutputBuffer); } } // Write non-blocking data to the USB line if (usbOutputBuffer != nullptr) { if (!SerialUSB) { // If the USB port is not opened, free the data left for writing OutputBuffer *buffer = usbOutputBuffer; usbOutputBuffer = nullptr; do { buffer = reprap.ReleaseOutput(buffer); } while (buffer != nullptr); } else { // Write as much data as we can... size_t bytesToWrite = min(SerialUSB.canWrite(), usbOutputBuffer->BytesLeft()); if (bytesToWrite > 0) { SerialUSB.write(usbOutputBuffer->Read(bytesToWrite), bytesToWrite); } if (usbOutputBuffer->BytesLeft() == 0) { usbOutputBuffer = reprap.ReleaseOutput(usbOutputBuffer); } } } // Thermostatically-controlled fans for (size_t fan = 0; fan < NUM_FANS; ++fan) { fans[fan].Check(); } // Diagnostics test if (debugCode == (int)DiagnosticTestType::TestSpinLockup) { for (;;) {} } ClassReport(longWait); } // Switch into boot mode and reset static void eraseAndReset() { cpu_irq_disable(); for(size_t i = 0; i <= (IFLASH_LAST_PAGE_ADDRESS - IFLASH_ADDR) / IFLASH_PAGE_SIZE; i++) { size_t pageStartAddr = IFLASH_ADDR + i * IFLASH_PAGE_SIZE; flash_unlock(pageStartAddr, pageStartAddr + IFLASH_PAGE_SIZE - 1, nullptr, nullptr); } flash_clear_gpnvm(1); // tell the system to boot from ROM next time rstc_start_software_reset(RSTC); for(;;) {} } void Platform::SoftwareReset(uint16_t reason) { if (reason == (uint16_t)SoftwareResetReason::erase) { eraseAndReset(); } else { if (reason != (uint16_t)SoftwareResetReason::user) { if (!SerialUSB.canWrite()) { reason |= (uint16_t)SoftwareResetReason::inUsbOutput; // if we are resetting because we are stuck in a Spin function, record whether we are trying to send to USB } if (reprap.GetNetwork()->InLwip()) { reason |= (uint16_t)SoftwareResetReason::inLwipSpin; } if (!Serial.canWrite() || !Serial1.canWrite()) { reason |= (uint16_t)SoftwareResetReason::inAuxOutput; // if we are resetting because we are stuck in a Spin function, record whether we are trying to send to aux } } reason |= (uint16_t)reprap.GetSpinningModule(); // Record the reason for the software reset SoftwareResetData temp; temp.magic = SoftwareResetData::magicValue; temp.resetReason = reason; GetStackUsage(NULL, NULL, &temp.neverUsedRam); if (reason != (uint16_t)SoftwareResetReason::user) { strncpy(temp.lastMessage, messageString.Pointer(), sizeof(temp.lastMessage) - 1); temp.lastMessage[sizeof(temp.lastMessage) - 1] = 0; } else { temp.lastMessage[0] = 0; } // Save diagnostics data to Flash and reset the software DueFlashStorage::write(SoftwareResetData::nvAddress, &temp, sizeof(SoftwareResetData)); } rstc_start_software_reset(RSTC); for(;;) {} } //***************************************************************************************************************** // Interrupts void TC3_Handler() { TC1->TC_CHANNEL[0].TC_IDR = TC_IER_CPAS; // disable the interrupt #ifdef MOVE_DEBUG ++numInterruptsExecuted; lastInterruptTime = Platform::GetInterruptClocks(); #endif reprap.Interrupt(); } void TC4_Handler() { TC_GetStatus(TC1, 1); reprap.GetNetwork()->Interrupt(); } void FanInterrupt() { ++fanInterruptCount; if (fanInterruptCount == fanMaxInterruptCount) { uint32_t now = micros(); fanInterval = now - fanLastResetTime; fanLastResetTime = now; fanInterruptCount = 0; } } void Platform::InitialiseInterrupts() { // Set the tick interrupt to the highest priority. We need to to monitor the heaters and kick the watchdog. NVIC_SetPriority(SysTick_IRQn, 0); // set priority for tick interrupts - highest, because it kicks the watchdog NVIC_SetPriority(UART_IRQn, 1); // set priority for UART interrupt - must be higher than step interrupt // Timer interrupt for stepper motors // The clock rate we use is a compromise. Too fast and the 64-bit square roots take a long time to execute. Too slow and we lose resolution. // We choose a clock divisor of 32, which gives us 0.38us resolution. The next option is 128 which would give 1.524us resolution. pmc_set_writeprotect(false); pmc_enable_periph_clk((uint32_t) TC3_IRQn); TC_Configure(TC1, 0, TC_CMR_WAVE | TC_CMR_WAVSEL_UP | TC_CMR_TCCLKS_TIMER_CLOCK3); TC1 ->TC_CHANNEL[0].TC_IDR = ~(uint32_t)0; // interrupts disabled for now TC_Start(TC1, 0); TC_GetStatus(TC1, 0); // clear any pending interrupt NVIC_SetPriority(TC3_IRQn, 2); // set high priority for this IRQ; it's time-critical NVIC_EnableIRQ(TC3_IRQn); // Timer interrupt to keep the networking timers running (called at 16Hz) pmc_enable_periph_clk((uint32_t) TC4_IRQn); TC_Configure(TC1, 1, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK2); uint32_t rc = (VARIANT_MCK/8)/16; // 8 because we selected TIMER_CLOCK2 above TC_SetRA(TC1, 1, rc/2); // 50% high, 50% low TC_SetRC(TC1, 1, rc); TC_Start(TC1, 1); TC1 ->TC_CHANNEL[1].TC_IER = TC_IER_CPCS; TC1 ->TC_CHANNEL[1].TC_IDR = ~TC_IER_CPCS; NVIC_SetPriority(TC4_IRQn, 4); // step interrupt is more time-critical than this one NVIC_EnableIRQ(TC4_IRQn); // Interrupt for 4-pin PWM fan sense line if (coolingFanRpmPin >= 0) { attachInterrupt(coolingFanRpmPin, FanInterrupt, FALLING); } // Tick interrupt for ADC conversions tickState = 0; currentHeater = 0; active = true; // this enables the tick interrupt, which keeps the watchdog happy } #if 0 // not used void Platform::DisableInterrupts() { NVIC_DisableIRQ(TC3_IRQn); NVIC_DisableIRQ(TC4_IRQn); } #endif //************************************************************************************************* // Debugging variables //extern "C" uint32_t longestWriteWaitTime, shortestWriteWaitTime, longestReadWaitTime, shortestReadWaitTime; //extern uint32_t maxRead, maxWrite; // This diagnostics function is the first to be called, so it calls Message to start with. // All other messages generated by this and other diagnostics functions must call AppendMessage. void Platform::Diagnostics() { Message(GENERIC_MESSAGE, "Platform Diagnostics:\n"); // Print memory stats and error codes to USB and copy them to the current webserver reply const char *ramstart = (char *) 0x20070000; const struct mallinfo mi = mallinfo(); Message(GENERIC_MESSAGE, "Memory usage:\n"); MessageF(GENERIC_MESSAGE, "Program static ram used: %d\n", &_end - ramstart); MessageF(GENERIC_MESSAGE, "Dynamic ram used: %d\n", mi.uordblks); MessageF(GENERIC_MESSAGE, "Recycled dynamic ram: %d\n", mi.fordblks); size_t currentStack, maxStack, neverUsed; GetStackUsage(¤tStack, &maxStack, &neverUsed); MessageF(GENERIC_MESSAGE, "Current stack ram used: %d\n", currentStack); MessageF(GENERIC_MESSAGE, "Maximum stack ram used: %d\n", maxStack); MessageF(GENERIC_MESSAGE, "Never used ram: %d\n", neverUsed); // Show the up time and reason for the last reset const uint32_t now = (uint32_t)Time(); // get up time in seconds const char* resetReasons[8] = { "power up", "backup", "watchdog", "software", "external", "?", "?", "?" }; MessageF(GENERIC_MESSAGE, "Last reset %02d:%02d:%02d ago, cause: %s\n", (unsigned int)(now/3600), (unsigned int)((now % 3600)/60), (unsigned int)(now % 60), resetReasons[(REG_RSTC_SR & RSTC_SR_RSTTYP_Msk) >> RSTC_SR_RSTTYP_Pos]); // Show the error code stored at the last software reset { SoftwareResetData temp; temp.magic = 0; DueFlashStorage::read(SoftwareResetData::nvAddress, &temp, sizeof(SoftwareResetData)); if (temp.magic == SoftwareResetData::magicValue) { MessageF(GENERIC_MESSAGE, "Last software reset code & available RAM: 0x%04x, %u\n", temp.resetReason, temp.neverUsedRam); MessageF(GENERIC_MESSAGE, "Spinning module during software reset: %s\n", moduleName[temp.resetReason & 0x0F]); if (temp.lastMessage[0]) { MessageF(GENERIC_MESSAGE, "Last message before reset: %s", temp.lastMessage); // usually ends with NL } } } // Show the current error codes MessageF(GENERIC_MESSAGE, "Error status: %u\n", errorCodeBits); // Show the current probe position heights Message(GENERIC_MESSAGE, "Bed probe heights:"); for (size_t i = 0; i < MAX_PROBE_POINTS; ++i) { MessageF(GENERIC_MESSAGE, " %.3f", reprap.GetMove()->ZBedProbePoint(i)); } Message(GENERIC_MESSAGE, "\n"); // Show the number of free entries in the file table unsigned int numFreeFiles = 0; for (size_t i = 0; i < MAX_FILES; i++) { if (!files[i]->inUse) { ++numFreeFiles; } } MessageF(GENERIC_MESSAGE, "Free file entries: %u\n", numFreeFiles); // Show the longest write time MessageF(GENERIC_MESSAGE, "Longest block write time: %.1fms\n", FileStore::GetAndClearLongestWriteTime()); // Debug //MessageF(GENERIC_MESSAGE, "Shortest/longest times read %.1f/%.1f write %.1f/%.1f ms, %u/%u\n", // (float)shortestReadWaitTime/1000, (float)longestReadWaitTime/1000, (float)shortestWriteWaitTime/1000, (float)longestWriteWaitTime/1000, // maxRead, maxWrite); //longestWriteWaitTime = longestReadWaitTime = 0; shortestReadWaitTime = shortestWriteWaitTime = 1000000; reprap.Timing(); #ifdef MOVE_DEBUG MessageF(GENERIC_MESSAGE, "Interrupts scheduled %u, done %u, last %u, next %u sched at %u, now %u\n", numInterruptsScheduled, numInterruptsExecuted, lastInterruptTime, nextInterruptTime, nextInterruptScheduledAt, GetInterruptClocks()); #endif } void Platform::DiagnosticTest(int d) { switch (d) { case (int)DiagnosticTestType::TestWatchdog: SysTick ->CTRL &= ~(SysTick_CTRL_TICKINT_Msk); // disable the system tick interrupt so that we get a watchdog timeout reset break; case (int)DiagnosticTestType::TestSpinLockup: debugCode = d; // tell the Spin function to loop break; case (int)DiagnosticTestType::TestSerialBlock: // write an arbitrary message via debugPrintf() debugPrintf("Diagnostic Test\n"); break; default: break; } } // Return the stack usage and amount of memory that has never been used, in bytes void Platform::GetStackUsage(size_t* currentStack, size_t* maxStack, size_t* neverUsed) const { const char *ramend = (const char *) 0x20088000; register const char * stack_ptr asm ("sp"); const char *heapend = sbrk(0); const char* stack_lwm = heapend; while (stack_lwm < stack_ptr && *stack_lwm == memPattern) { ++stack_lwm; } if (currentStack) { *currentStack = ramend - stack_ptr; } if (maxStack) { *maxStack = ramend - stack_lwm; } if (neverUsed) { *neverUsed = stack_lwm - heapend; } } void Platform::ClassReport(float &lastTime) { const Module spinningModule = reprap.GetSpinningModule(); if (reprap.Debug(spinningModule)) { if (Time() - lastTime >= LONG_TIME) { lastTime = Time(); MessageF(HOST_MESSAGE, "Class %s spinning.\n", moduleName[spinningModule]); } } } //=========================================================================== //=============================Thermal Settings ============================ //=========================================================================== // See http://en.wikipedia.org/wiki/Thermistor#B_or_.CE.B2_parameter_equation // BETA is the B value // RS is the value of the series resistor in ohms // R_INF is R0.exp(-BETA/T0), where R0 is the thermistor resistance at T0 (T0 is in kelvin) // Normally T0 is 298.15K (25 C). If you write that expression in brackets in the #define the compiler // should compute it for you (i.e. it won't need to be calculated at run time). // If the A->D converter has a range of 0..1023 and the measured voltage is V (between 0 and 1023) // then the thermistor resistance, R = V.RS/(1024 - V) // and the temperature, T = BETA/ln(R/R_INF) // To get degrees celsius (instead of kelvin) add -273.15 to T // Result is in degrees celsius float Platform::GetTemperature(size_t heater) const { int rawTemp = GetRawTemperature(heater); // If the ADC reading is N then for an ideal ADC, the input voltage is at least N/(AD_RANGE + 1) and less than (N + 1)/(AD_RANGE + 1), times the analog reference. // So we add 0.5 to to the reading to get a better estimate of the input. float reading = (float) rawTemp + 0.5; // Recognise the special case of thermistor disconnected. // For some ADCs, the high-end offset is negative, meaning that the ADC never returns a high enough value. We need to allow for this here. const PidParameters& p = nvData.pidParams[heater]; if (p.adcHighOffset < 0.0) { rawTemp -= (int) p.adcHighOffset; } if (rawTemp >= (int)AD_DISCONNECTED_VIRTUAL) { return ABS_ZERO; // thermistor is disconnected } // Correct for the low and high ADC offsets reading -= p.adcLowOffset; reading *= (AD_RANGE_VIRTUAL + 1) / (AD_RANGE_VIRTUAL + 1 + p.adcHighOffset - p.adcLowOffset); float resistance = reading * p.thermistorSeriesR / ((AD_RANGE_VIRTUAL + 1) - reading); return (resistance <= p.GetRInf()) ? 2000.0 // thermistor short circuit, return a high temperature : ABS_ZERO + p.GetBeta() / log(resistance / p.GetRInf()); } // See if we need to turn on the hot end fan bool Platform::AnyHeaterHot(uint16_t heaters, float t) const { for (size_t h = 0; h < reprap.GetHeatersInUse(); ++h) { if (((1 << h) & heaters) != 0) { const float ht = GetTemperature(h); if (ht >= t || ht < BAD_LOW_TEMPERATURE) { return true; } } } return false; } void Platform::SetPidParameters(size_t heater, const PidParameters& params) { if (heater < HEATERS && params != nvData.pidParams[heater]) { nvData.pidParams[heater] = params; if (autoSaveEnabled) { WriteNvData(); } } } const PidParameters& Platform::GetPidParameters(size_t heater) const { return nvData.pidParams[heater]; } // power is a fraction in [0,1] void Platform::SetHeater(size_t heater, float power) { SetHeaterPwm(heater, (uint8_t)(255.0 * min(1.0, max(0.0, power)))); } void Platform::SetHeaterPwm(size_t heater, uint8_t power) { if (heatOnPins[heater] >= 0) { uint16_t freq = (reprap.GetHeat()->UseSlowPwm(heater)) ? SlowHeaterPwmFreq : NormalHeaterPwmFreq; analogWriteNonDue(heatOnPins[heater], (HEAT_ON == 0) ? 255 - power : power, freq); } } EndStopHit Platform::Stopped(size_t drive) const { if (endStopType[drive] == EndStopType::noEndStop) { // No homing switch is configured for this axis, so see if we should use the Z probe if (nvData.zProbeType > 0 && drive < AXES && nvData.zProbeAxes[drive]) { return GetZProbeResult(); // using the Z probe as a low homing stop for this axis, so just get its result } } else if (endStopPins[drive] >= 0) { if (digitalReadNonDue(endStopPins[drive]) == ((endStopLogicLevel[drive]) ? 1 : 0)) { return (endStopType[drive] == EndStopType::highEndStop) ? EndStopHit::highHit : EndStopHit::lowHit; } } return EndStopHit::noStop; } // Return the Z probe result. We assume that if the Z probe is used as an endstop, it is used as the low stop. EndStopHit Platform::GetZProbeResult() const { const int zProbeVal = ZProbe(); const int zProbeADValue = (nvData.zProbeType == 4) ? nvData.switchZProbeParameters.adcValue : (nvData.zProbeType == 3) ? nvData.alternateZProbeParameters.adcValue : nvData.irZProbeParameters.adcValue; return (zProbeVal >= zProbeADValue) ? EndStopHit::lowHit : (zProbeVal * 10 >= zProbeADValue * 9) ? EndStopHit::lowNear // if we are at/above 90% of the target value : EndStopHit::noStop; } // This is called from the step ISR as well as other places, so keep it fast, especially in the case where the motor is already enabled void Platform::SetDirection(size_t drive, bool direction) { const int driver = driverNumbers[drive]; if (driver >= 0) { const int pin = directionPins[driver]; if (pin >= 0) { bool d = (direction == FORWARDS) ? directions[driver] : !directions[driver]; digitalWriteNonDue(pin, d); } } } // Enable a drive. Must not be called from an ISR, or with interrupts disabled. void Platform::EnableDrive(size_t drive) { if (drive < DRIVES && driveState[drive] != DriveStatus::enabled) { driveState[drive] = DriveStatus::enabled; const size_t driver = driverNumbers[drive]; if (driver >= 0) { UpdateMotorCurrent(driver); // the current may have been reduced by the idle timeout const int pin = enablePins[driver]; if (pin >= 0) { digitalWriteNonDue(pin, enableValues[driver]); } } } } // Disable a drive, if it has a disable pin void Platform::DisableDrive(size_t drive) { if (drive < DRIVES) { const size_t driver = driverNumbers[drive]; const int pin = enablePins[driver]; if (pin >= 0) { digitalWriteNonDue(pin, !enableValues[driver]); } driveState[drive] = DriveStatus::disabled; } } // Set drives to idle hold if they are enabled. If a drive is disabled, leave it alone. // Must not be called from an ISR, or with interrupts disabled. void Platform::SetDrivesIdle() { for (size_t drive = 0; drive < DRIVES; ++drive) { if (driveState[drive] == DriveStatus::enabled) { driveState[drive] = DriveStatus::idle; UpdateMotorCurrent(drive); } } } // Set the current for a motor. Current is in mA. void Platform::SetMotorCurrent(size_t drive, float current) { if (drive < DRIVES) { motorCurrents[drive] = current; UpdateMotorCurrent(drive); } } // This must not be called from an ISR, or with interrupts disabled. void Platform::UpdateMotorCurrent(size_t drive) { if (drive < DRIVES) { float current = motorCurrents[drive]; if (driveState[drive] == DriveStatus::idle) { current *= idleCurrentFactor; } unsigned short pot = (unsigned short)((0.256*current*8.0*senseResistor + maxStepperDigipotVoltage/2)/maxStepperDigipotVoltage); unsigned short dac = (unsigned short)((0.256*current*8.0*senseResistor + maxStepperDACVoltage/2)/maxStepperDACVoltage); const size_t driver = driverNumbers[drive]; if (driver < 4) { mcpDuet.setNonVolatileWiper(potWipes[driver], pot); mcpDuet.setVolatileWiper(potWipes[driver], pot); } else { if (board == BoardType::Duet_085) { // Extruder 0 is on DAC channel 0 if (driver == 4) { analogWrite(DAC0, dac); } else { mcpExpansion.setNonVolatileWiper(potWipes[driver-1], pot); mcpExpansion.setVolatileWiper(potWipes[driver-1], pot); } } else { mcpExpansion.setNonVolatileWiper(potWipes[driver], pot); mcpExpansion.setVolatileWiper(potWipes[driver], pot); } } } } float Platform::MotorCurrent(size_t drive) const { return (drive < DRIVES) ? motorCurrents[drive] : 0.0; } // Set the motor idle current factor void Platform::SetIdleCurrentFactor(float f) { idleCurrentFactor = f; for (size_t drive = 0; drive < DRIVES; ++drive) { if (driveState[drive] == DriveStatus::idle) { UpdateMotorCurrent(drive); } } } // Set the physical drive (i.e. axis or extruder) number used by this driver void Platform::SetPhysicalDrive(size_t driverNumber, int8_t physicalDrive) { int oldDrive = GetPhysicalDrive(driverNumber); if (oldDrive >= 0) { driverNumbers[oldDrive] = -1; stepPinDescriptors[oldDrive] = OutputPin(); } driverNumbers[physicalDrive] = driverNumber; stepPinDescriptors[physicalDrive] = OutputPin(stepPins[driverNumber]); } // Return the physical drive used by this driver, or -1 if not found int Platform::GetPhysicalDrive(size_t driverNumber) const { for (size_t drive = 0; drive < DRIVES; ++drive) { if (driverNumbers[drive] == (int8_t)driverNumber) { return drive; } } return -1; } // Get current cooling fan speed on a scale between 0 and 1 float Platform::GetFanValue(size_t fan) const { return (fan < NUM_FANS) ? fans[fan].val : -1; } bool Platform::GetCoolingInverted(size_t fan) const { return (fan < NUM_FANS) ? fans[fan].inverted : -1; } void Platform::SetCoolingInverted(size_t fan, bool inv) { if (fan < NUM_FANS) { fans[fan].inverted = inv; } } // This is a bit of a compromise - old RepRaps used fan speeds in the range // [0, 255], which is very hardware dependent. It makes much more sense // to specify speeds in [0.0, 1.0]. This looks at the value supplied (which // the G Code reader will get right for a float or an int) and attempts to // do the right thing whichever the user has done. void Platform::SetFanValue(size_t fan, float speed) { if (fan < NUM_FANS) { fans[fan].SetValue(speed); } } // Get current fan RPM float Platform::GetFanRPM() { // The ISR sets fanInterval to the number of microseconds it took to get fanMaxInterruptCount interrupts. // We get 2 tacho pulses per revolution, hence 2 interrupts per revolution. // However, if the fan stops then we get no interrupts and fanInterval stops getting updated. // We must recognise this and return zero. return (fanInterval != 0 && micros() - fanLastResetTime < 3000000U) // if we have a reading and it is less than 3 second old ? (float)((30000000U * fanMaxInterruptCount)/fanInterval) // then calculate RPM assuming 2 interrupts per rev : 0.0; // else assume fan is off or tacho not connected } void Platform::InitFans() { for (size_t i = 0; i < NUM_FANS; ++i) { // The cooling fan 0 output pin gets inverted if HEAT_ON == 0 on a Duet 0.4, 0.6 or 0.7 fans[i].Init(COOLING_FAN_PINS[i], !HEAT_ON && board != BoardType::Duet_085); } if (NUM_FANS > 1) { // Set fan 1 to be thermostatic by default, monitoring all heaters except the default bed heater fans[1].SetHeatersMonitored(0xFFFF & ~(1 << BED_HEATER)); } coolingFanRpmPin = COOLING_FAN_RPM_PIN; lastRpmResetTime = 0.0; if (coolingFanRpmPin >= 0) { pinModeNonDue(coolingFanRpmPin, INPUT_PULLUP, 1500); // enable pullup and 1500Hz debounce filter (500Hz only worked up to 7000RPM) } } float Platform::GetFanPwmFrequency(size_t fan) const { if (fan < NUM_FANS) { return (float)fans[fan].freq; } return 0.0; } void Platform::SetFanPwmFrequency(size_t fan, float freq) { if (fan < NUM_FANS) { fans[fan].SetPwmFrequency(freq); } } float Platform::GetTriggerTemperature(size_t fan) const { if (fan < NUM_FANS) { return fans[fan].triggerTemperature; } return ABS_ZERO; } void Platform::SetTriggerTemperature(size_t fan, float t) { if (fan < NUM_FANS) { fans[fan].SetTriggerTemperature(t); } } uint16_t Platform::GetHeatersMonitored(size_t fan) const { if (fan < NUM_FANS) { return fans[fan].heatersMonitored; } return 0; } void Platform::SetHeatersMonitored(size_t fan, uint16_t h) { if (fan < NUM_FANS) { fans[fan].SetHeatersMonitored(h); } } void Platform::Fan::Init(Pin p_pin, bool hwInverted) { val = 0.0; freq = DefaultFanPwmFreq; pin = p_pin; hardwareInverted = hwInverted; inverted = false; heatersMonitored = 0; triggerTemperature = HOT_END_FAN_TEMPERATURE; Refresh(); } void Platform::Fan::SetValue(float speed) { if (heatersMonitored == 0) { if (speed > 1.0) { speed /= 255.0; } val = constrain(speed, 0.0, 1.0); Refresh(); } } void Platform::Fan::Refresh() { if (pin >= 0) { uint32_t p = (uint32_t)(255.0 * val); bool invert = hardwareInverted; if (inverted) { invert = !invert; } analogWriteNonDue(pin, (invert) ? (255 - p) : p, freq); } } void Platform::Fan::SetPwmFrequency(float p_freq) { freq = (uint16_t)max(1.0, min(65535.0, p_freq)); Refresh(); } void Platform::Fan::Check() { if (heatersMonitored != 0) { val = (reprap.GetPlatform()->AnyHeaterHot(heatersMonitored, triggerTemperature)) ? 1.0 : 0.0; Refresh(); } } void Platform::SetMACAddress(uint8_t mac[]) { bool changed = false; for (size_t i = 0; i < 6; i++) { if (nvData.macAddress[i] != mac[i]) { nvData.macAddress[i] = mac[i]; changed = true; } } if (changed && autoSaveEnabled) { WriteNvData(); } } //----------------------------------------------------------------------------------------------------- FileStore* Platform::GetFileStore(const char* directory, const char* fileName, bool write) { if (!fileStructureInitialised) return NULL; for (size_t i = 0; i < MAX_FILES; i++) { if (!files[i]->inUse) { files[i]->inUse = true; if (files[i]->Open(directory, fileName, write)) { return files[i]; } else { files[i]->inUse = false; return NULL; } } } Message(HOST_MESSAGE, "Max open file count exceeded.\n"); return NULL; } MassStorage* Platform::GetMassStorage() { return massStorage; } void Platform::Message(MessageType type, const char *message) { switch (type) { case FLASH_LED: // Message that is to flash an LED; the next two bytes define // the frequency and M/S ratio. // (not implemented yet) break; case AUX_MESSAGE: // Message that is to be sent to the first auxiliary device if (auxOutputBuffer != nullptr) { // If we're still busy sending a response to the UART device, append this message to the output buffer auxOutputBuffer->cat(message); } else { // Send the beep command to the aux channel. There is no flow control on this port, so it can't block for long. Serial.write(message); Serial.flush(); } break; case AUX2_MESSAGE: // Message that is to be sent to the second auxiliary device (blocking) Serial1.write(message); Serial1.flush(); break; case DISPLAY_MESSAGE: // Message that is to appear on a local display; \f and \n should be supported. reprap.SetMessage(message); break; case DEBUG_MESSAGE: // Debug messages in blocking mode - potentially DANGEROUS, use with care! SerialUSB.write(message); SerialUSB.flush(); break; case HOST_MESSAGE: // Message that is to be sent via the USB line (non-blocking) // // Ensure we have a valid buffer to write to if (usbOutputBuffer == nullptr) { OutputBuffer *buffer; if (!reprap.AllocateOutput(buffer)) { // Should never happen return; } usbOutputBuffer = buffer; } // Check if we need to write the indentation chars first { const size_t stackPointer = reprap.GetGCodes()->GetStackPointer(); if (stackPointer > 0) { // First, make sure we get the indentation right char indentation[StackSize * 2 + 1]; for(size_t i = 0; i < stackPointer * 2; i++) { indentation[i] = ' '; } indentation[stackPointer * 2] = 0; // Append the indentation string to our chain, or allocate a new buffer if there is none usbOutputBuffer->cat(indentation); } } // Append the message string to the output buffer chain usbOutputBuffer->cat(message); break; case HTTP_MESSAGE: case TELNET_MESSAGE: // Message that is to be sent to the web { const WebSource source = (type == HTTP_MESSAGE) ? WebSource::HTTP : WebSource::Telnet; reprap.GetWebserver()->HandleGCodeReply(source, message); } break; case GENERIC_MESSAGE: // Message that is to be sent to the web & host. Make this the default one, too. default: Message(HOST_MESSAGE, message); Message(HTTP_MESSAGE, message); Message(TELNET_MESSAGE, message); break; } } void Platform::Message(const MessageType type, const StringRef &message) { Message(type, message.Pointer()); } void Platform::Message(const MessageType type, OutputBuffer *buffer) { switch (type) { case AUX_MESSAGE: // If no AUX device is attached, don't queue this buffer if (!reprap.GetGCodes()->HaveAux()) { while (buffer != nullptr) { buffer = reprap.ReleaseOutput(buffer); } break; } // For big responses it makes sense to write big chunks of data in portions. Store this data here if (auxOutputBuffer == nullptr) { auxOutputBuffer = buffer; } else { auxOutputBuffer->Append(buffer); } break; case AUX2_MESSAGE: // Send this message to the second UART device if (aux2OutputBuffer == nullptr) { aux2OutputBuffer = buffer; } else { aux2OutputBuffer->Append(buffer); } break; case DEBUG_MESSAGE: // Probably rarely used, but supported. while (buffer != nullptr) { SerialUSB.write(buffer->Data(), buffer->DataLength()); SerialUSB.flush(); buffer = reprap.ReleaseOutput(buffer); } break; case HOST_MESSAGE: // If the serial USB line is not open, discard its content right away if (!SerialUSB) { while (buffer != nullptr) { buffer = reprap.ReleaseOutput(buffer); } } else { // Append incoming data to the list of our output buffers if (usbOutputBuffer == nullptr) { usbOutputBuffer = buffer; } else { usbOutputBuffer->Append(buffer); } } break; case HTTP_MESSAGE: case TELNET_MESSAGE: // Message that is to be sent to the web { const WebSource source = (type == HTTP_MESSAGE) ? WebSource::HTTP : WebSource::Telnet; reprap.GetWebserver()->HandleGCodeReply(source, buffer); } break; case GENERIC_MESSAGE: // Message that is to be sent to the web & host. buffer->IncreaseReferences(2); // This one is handled by two additional destinations Message(HOST_MESSAGE, buffer); Message(HTTP_MESSAGE, buffer); Message(TELNET_MESSAGE, buffer); break; default: // Everything else is unsupported (and probably not used) MessageF(HOST_MESSAGE, "Warning: Unsupported Message call for type %u!\n", type); break; } } void Platform::MessageF(MessageType type, const char *fmt, va_list vargs) { char formatBuffer[FORMAT_STRING_LENGTH]; StringRef formatString(formatBuffer, ARRAY_SIZE(formatBuffer)); formatString.vprintf(fmt, vargs); Message(type, formatBuffer); } void Platform::MessageF(MessageType type, const char *fmt, ...) { char formatBuffer[FORMAT_STRING_LENGTH]; StringRef formatString(formatBuffer, ARRAY_SIZE(formatBuffer)); va_list vargs; va_start(vargs, fmt); formatString.vprintf(fmt, vargs); va_end(vargs); Message(type, formatBuffer); } bool Platform::AtxPower() const { return (digitalReadNonDue(atxPowerPin) == HIGH); } void Platform::SetAtxPower(bool on) { digitalWriteNonDue(atxPowerPin, (on) ? HIGH : LOW); } void Platform::SetElasticComp(size_t drive, float factor) { if (drive < DRIVES - AXES) { elasticComp[drive] = factor; } } float Platform::ActualInstantDv(size_t drive) const { float idv = instantDvs[drive]; if (drive >= AXES) { float eComp = elasticComp[drive - AXES]; // If we are using elastic compensation then we need to limit the instantDv to avoid velocity mismatches return (eComp <= 0.0) ? idv : min(idv, 1.0/(eComp * driveStepsPerUnit[drive])); } else { return idv; } } void Platform::SetBaudRate(size_t chan, uint32_t br) { if (chan < NUM_SERIAL_CHANNELS) { baudRates[chan] = br; ResetChannel(chan); } } uint32_t Platform::GetBaudRate(size_t chan) const { return (chan < NUM_SERIAL_CHANNELS) ? baudRates[chan] : 0; } void Platform::SetCommsProperties(size_t chan, uint32_t cp) { if (chan < NUM_SERIAL_CHANNELS) { commsParams[chan] = cp; ResetChannel(chan); } } uint32_t Platform::GetCommsProperties(size_t chan) const { return (chan < NUM_SERIAL_CHANNELS) ? commsParams[chan] : 0; } // Re-initialise a serial channel. // Ideally, this would be part of the Line class. However, the Arduino core inexplicably fails to make the serial I/O begin() and end() members // virtual functions of a base class, which makes that difficult to do. void Platform::ResetChannel(size_t chan) { switch(chan) { case 0: SerialUSB.end(); SerialUSB.begin(baudRates[0]); break; case 1: Serial.end(); Serial.begin(baudRates[1]); break; default: break; } } void Platform::SetBoardType(BoardType bt) { if (bt == BoardType::Auto) { // Determine whether this is a Duet 0.6 or a Duet 0.8.5 board. // If it is a 0.85 board then DAC0 (AKA digital pin 67) is connected to ground via a diode and a 2.15K resistor. // So we enable the pullup (value 150K-150K) on pin 67 and read it, expecting a LOW on a 0.8.5 board and a HIGH on a 0.6 board. // This may fail if anyone connects a load to the DAC0 pin on and Dur=et 0.6, hence we implement board selection in M115 as well. pinModeNonDue(Dac0DigitalPin, INPUT_PULLUP); board = (digitalReadNonDue(Dac0DigitalPin)) ? BoardType::Duet_06 : BoardType::Duet_085; pinModeNonDue(Dac0DigitalPin, INPUT); // turn pullup off } else { board = bt; } if (active) { InitZProbe(); // select and initialise the Z probe modulation pin InitFans(); // select whether cooling is inverted or not } } // Get a string describing the electronics const char* Platform::GetElectronicsString() const { switch (board) { case BoardType::Duet_06: return "Duet 0.6"; case BoardType::Duet_07: return "Duet 0.7"; case BoardType::Duet_085: return "Duet 0.85"; default: return "Unidentified"; } } // Direct pin operations // Set the specified pin to the specified output level. Return true if success, false if not allowed. bool Platform::SetPin(int pin, int level) { if (pin >= 0 && (unsigned int)pin < numPins && (level == 0 || level == 1)) { const size_t index = (unsigned int)pin/8; const uint8_t mask = 1 << ((unsigned int)pin & 7); if ((pinAccessAllowed[index] & mask) != 0) { if ((pinInitialised[index] & mask) == 0) { pinModeNonDue(pin, OUTPUT); pinInitialised[index] |= mask; } digitalWriteNonDue(pin, level); return true; } } return false; } bool Platform::GCodeAvailable(const SerialSource source) const { switch (source) { case SerialSource::USB: return SerialUSB.available() > 0; case SerialSource::AUX: return Serial.available() > 0; case SerialSource::AUX2: return Serial1.available() > 0; } return false; } char Platform::ReadFromSource(const SerialSource source) { switch (source) { case SerialSource::USB: return static_cast(SerialUSB.read()); case SerialSource::AUX: return static_cast(Serial.read()); case SerialSource::AUX2: return static_cast(Serial1.read()); } return 0; } // Pragma pop_options is not supported on this platform, so we put this time-critical code right at the end of the file //#pragma GCC push_options #pragma GCC optimize ("O3") // Schedule an interrupt at the specified clock count, or return true if that time is imminent or has passed already. // Must be called with interrupts disabled, /*static*/ bool Platform::ScheduleInterrupt(uint32_t tim) { TC_SetRA(TC1, 0, tim); // set up the compare register TC_GetStatus(TC1, 0); // clear any pending interrupt int32_t diff = (int32_t)(tim - TC_ReadCV(TC1, 0)); // see how long we have to go if (diff < (int32_t)DDA::minInterruptInterval) // if less than about 2us or already passed { return true; // tell the caller to simulate an interrupt instead } TC1 ->TC_CHANNEL[0].TC_IER = TC_IER_CPAS; // enable the interrupt #ifdef MOVE_DEBUG ++numInterruptsScheduled; nextInterruptTime = tim; nextInterruptScheduledAt = Platform::GetInterruptClocks(); #endif return false; } // Process a 1ms tick interrupt // This function must be kept fast so as not to disturb the stepper timing, so don't do any floating point maths in here. // This is what we need to do: // 0. Kick the watchdog. // 1. Kick off a new ADC conversion. // 2. Fetch and process the result of the last ADC conversion. // 3a. If the last ADC conversion was for the Z probe, toggle the modulation output if using a modulated IR sensor. // 3b. If the last ADC reading was a thermistor reading, check for an over-temperature situation and turn off the heater if necessary. // We do this here because the usual polling loop sometimes gets stuck trying to send data to the USB port. //#define TIME_TICK_ISR 1 // define this to store the tick ISR time in errorCodeBits void Platform::Tick() { #ifdef TIME_TICK_ISR uint32_t now = micros(); #endif switch (tickState) { case 1: // last conversion started was a thermistor case 3: { ThermistorAveragingFilter& currentFilter = const_cast(thermistorFilters[currentHeater]); currentFilter.ProcessReading(GetAdcReading(heaterAdcChannels[currentHeater])); StartAdcConversion(zProbeAdcChannel); if (currentFilter.IsValid()) { uint32_t sum = currentFilter.GetSum(); if (sum < thermistorOverheatSums[currentHeater] || sum >= AD_DISCONNECTED_REAL * THERMISTOR_AVERAGE_READINGS) { // We have an over-temperature or disconnected reading from this thermistor, so turn off the heater SetHeaterPwm(currentHeater, 0); LogError(ErrorCode::BadTemp); } } ++currentHeater; if (currentHeater == HEATERS) { currentHeater = 0; } } ++tickState; break; case 2: // last conversion started was the Z probe, with IR LED on const_cast(zProbeOnFilter).ProcessReading(GetRawZProbeReading()); StartAdcConversion(heaterAdcChannels[currentHeater]); // read a thermistor if (nvData.zProbeType == 2) // if using a modulated IR sensor { digitalWriteNonDue(zProbeModulationPin, LOW); // turn off the IR emitter } ++tickState; break; case 4: // last conversion started was the Z probe, with IR LED off if modulation is enabled const_cast(zProbeOffFilter).ProcessReading(GetRawZProbeReading()); // no break case 0: // this is the state after initialisation, no conversion has been started default: StartAdcConversion(heaterAdcChannels[currentHeater]); // read a thermistor if (nvData.zProbeType == 2) // if using a modulated IR sensor { digitalWriteNonDue(zProbeModulationPin, HIGH); // turn on the IR emitter } tickState = 1; break; } #ifdef TIME_TICK_ISR uint32_t now2 = micros(); if (now2 - now > errorCodeBits) { errorCodeBits = now2 - now; } #endif } /*static*/ uint16_t Platform::GetAdcReading(adc_channel_num_t chan) { uint16_t rslt = (uint16_t) adc_get_channel_value(ADC, chan); adc_disable_channel(ADC, chan); return rslt; } /*static*/ void Platform::StartAdcConversion(adc_channel_num_t chan) { adc_enable_channel(ADC, chan); adc_start(ADC ); } // Pragma pop_options is not supported on this platform //#pragma GCC pop_options // End