diff options
Diffstat (limited to 'Marlin/src/libs')
34 files changed, 5749 insertions, 0 deletions
diff --git a/Marlin/src/libs/BL24CXX.cpp b/Marlin/src/libs/BL24CXX.cpp new file mode 100644 index 0000000..8de320d --- /dev/null +++ b/Marlin/src/libs/BL24CXX.cpp @@ -0,0 +1,272 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "BL24CXX.h" +#include <libmaple/gpio.h> + +#ifndef EEPROM_WRITE_DELAY + #define EEPROM_WRITE_DELAY 10 +#endif +#ifndef EEPROM_DEVICE_ADDRESS + #define EEPROM_DEVICE_ADDRESS (0x50 << 1) +#endif + +// IO direction setting +#define SDA_IN() do{ PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH &= 0XFFFF0FFF; PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH |= 8 << 12; }while(0) +#define SDA_OUT() do{ PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH &= 0XFFFF0FFF; PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH |= 3 << 12; }while(0) + +// IO ops +#define IIC_SCL_0() WRITE(IIC_EEPROM_SCL, LOW) +#define IIC_SCL_1() WRITE(IIC_EEPROM_SCL, HIGH) +#define IIC_SDA_0() WRITE(IIC_EEPROM_SDA, LOW) +#define IIC_SDA_1() WRITE(IIC_EEPROM_SDA, HIGH) +#define READ_SDA() READ(IIC_EEPROM_SDA) + +// +// Simple IIC interface via libmaple +// + +// Initialize IIC +void IIC::init() { + SET_OUTPUT(IIC_EEPROM_SDA); + SET_OUTPUT(IIC_EEPROM_SCL); + IIC_SCL_1(); + IIC_SDA_1(); +} + +// Generate IIC start signal +void IIC::start() { + SDA_OUT(); // SDA line output + IIC_SDA_1(); + IIC_SCL_1(); + delay_us(4); + IIC_SDA_0(); // START:when CLK is high, DATA change from high to low + delay_us(4); + IIC_SCL_0(); // Clamp the I2C bus, ready to send or receive data +} + +// Generate IIC stop signal +void IIC::stop() { + SDA_OUT(); // SDA line output + IIC_SCL_0(); + IIC_SDA_0(); // STOP:when CLK is high DATA change from low to high + delay_us(4); + IIC_SCL_1(); + IIC_SDA_1(); // Send I2C bus end signal + delay_us(4); +} + +// Wait for the response signal to arrive +// 1 = failed to receive response +// 0 = response received +uint8_t IIC::wait_ack() { + uint8_t ucErrTime = 0; + SDA_IN(); // SDA is set as input + IIC_SDA_1(); delay_us(1); + IIC_SCL_1(); delay_us(1); + while (READ_SDA()) { + if (++ucErrTime > 250) { + stop(); + return 1; + } + } + IIC_SCL_0(); // Clock output 0 + return 0; +} + +// Generate ACK response +void IIC::ack() { + IIC_SCL_0(); + SDA_OUT(); + IIC_SDA_0(); + delay_us(2); + IIC_SCL_1(); + delay_us(2); + IIC_SCL_0(); +} + +// No ACK response +void IIC::nAck() { + IIC_SCL_0(); + SDA_OUT(); + IIC_SDA_1(); + delay_us(2); + IIC_SCL_1(); + delay_us(2); + IIC_SCL_0(); +} + +// Send one IIC byte +// Return whether the slave responds +// 1 = there is a response +// 0 = no response +void IIC::send_byte(uint8_t txd) { + SDA_OUT(); + IIC_SCL_0(); // Pull down the clock to start data transmission + LOOP_L_N(t, 8) { + // IIC_SDA = (txd & 0x80) >> 7; + if (txd & 0x80) IIC_SDA_1(); else IIC_SDA_0(); + txd <<= 1; + delay_us(2); // All three delays are necessary for TEA5767 + IIC_SCL_1(); + delay_us(2); + IIC_SCL_0(); + delay_us(2); + } +} + +// Read 1 byte, when ack=1, send ACK, ack=0, send nACK +uint8_t IIC::read_byte(unsigned char ack_chr) { + unsigned char receive = 0; + SDA_IN(); // SDA is set as input + LOOP_L_N(i, 8) { + IIC_SCL_0(); + delay_us(2); + IIC_SCL_1(); + receive <<= 1; + if (READ_SDA()) receive++; + delay_us(1); + } + ack_chr ? ack() : nAck(); // Send ACK / send nACK + return receive; +} + +/******************** EEPROM ********************/ + +// Initialize the IIC interface +void BL24CXX::init() { IIC::init(); } + +// Read a byte at the specified address +// ReadAddr: the address to start reading +// Return: the byte read +uint8_t BL24CXX::readOneByte(uint16_t ReadAddr) { + uint8_t temp = 0; + IIC::start(); + if (EE_TYPE > BL24C16) { + IIC::send_byte(EEPROM_DEVICE_ADDRESS); // Send write command + IIC::wait_ack(); + IIC::send_byte(ReadAddr >> 8); // Send high address + IIC::wait_ack(); + } + else + IIC::send_byte(EEPROM_DEVICE_ADDRESS + ((ReadAddr >> 8) << 1)); // Send device address 0xA0, write data + + IIC::wait_ack(); + IIC::send_byte(ReadAddr & 0xFF); // Send low address + IIC::wait_ack(); + IIC::start(); + IIC::send_byte(EEPROM_DEVICE_ADDRESS | 0x01); // Send byte + IIC::wait_ack(); + temp = IIC::read_byte(0); + IIC::stop(); // Generate a stop condition + return temp; +} + +// Write a data at the address specified by BL24CXX +// WriteAddr: The destination address for writing data +// DataToWrite: the data to be written +void BL24CXX::writeOneByte(uint16_t WriteAddr, uint8_t DataToWrite) { + IIC::start(); + if (EE_TYPE > BL24C16) { + IIC::send_byte(EEPROM_DEVICE_ADDRESS); // Send write command + IIC::wait_ack(); + IIC::send_byte(WriteAddr >> 8); // Send high address + } + else + IIC::send_byte(EEPROM_DEVICE_ADDRESS + ((WriteAddr >> 8) << 1)); // Send device address 0xA0, write data + + IIC::wait_ack(); + IIC::send_byte(WriteAddr & 0xFF); // Send low address + IIC::wait_ack(); + IIC::send_byte(DataToWrite); // Receiving mode + IIC::wait_ack(); + IIC::stop(); // Generate a stop condition + delay(10); +} + +// Start writing data of length Len at the specified address in BL24CXX +// This function is used to write 16bit or 32bit data. +// WriteAddr: the address to start writing +// DataToWrite: the first address of the data array +// Len: The length of the data to be written 2, 4 +void BL24CXX::writeLenByte(uint16_t WriteAddr, uint32_t DataToWrite, uint8_t Len) { + LOOP_L_N(t, Len) + writeOneByte(WriteAddr + t, (DataToWrite >> (8 * t)) & 0xFF); +} + +// Start reading data of length Len from the specified address in BL24CXX +// This function is used to read 16bit or 32bit data. +// ReadAddr: the address to start reading +// Return value: data +// Len: The length of the data to be read 2,4 +uint32_t BL24CXX::readLenByte(uint16_t ReadAddr, uint8_t Len) { + uint32_t temp = 0; + LOOP_L_N(t, Len) { + temp <<= 8; + temp += readOneByte(ReadAddr + Len - t - 1); + } + return temp; +} + +// Check if BL24CXX is normal +// Return 1: Detection failed +// return 0: detection is successful +#define BL24CXX_TEST_ADDRESS 0x00 +#define BL24CXX_TEST_VALUE 0x55 + +bool BL24CXX::_check() { + return (readOneByte(BL24CXX_TEST_ADDRESS) != BL24CXX_TEST_VALUE); // false = success! +} + +bool BL24CXX::check() { + if (_check()) { // Value was written? Good EEPROM! + writeOneByte(BL24CXX_TEST_ADDRESS, BL24CXX_TEST_VALUE); // Write now and check. + return _check(); + } + return false; // success! +} + +// Start reading the specified number of data at the specified address in BL24CXX +// ReadAddr: The address to start reading is 0~255 for 24c02 +// pBuffer: the first address of the data array +// NumToRead: the number of data to be read +void BL24CXX::read(uint16_t ReadAddr, uint8_t *pBuffer, uint16_t NumToRead) { + for (; NumToRead; NumToRead--) + *pBuffer++ = readOneByte(ReadAddr++); +} + +// Start writing the specified number of data at the specified address in BL24CXX +// WriteAddr: the address to start writing, 0~255 for 24c02 +// pBuffer: the first address of the data array +// NumToWrite: the number of data to be written +void BL24CXX::write(uint16_t WriteAddr, uint8_t *pBuffer, uint16_t NumToWrite) { + for (; NumToWrite; NumToWrite--, WriteAddr++) + writeOneByte(WriteAddr, *pBuffer++); +} + +#endif // IIC_BL24CXX_EEPROM diff --git a/Marlin/src/libs/BL24CXX.h b/Marlin/src/libs/BL24CXX.h new file mode 100644 index 0000000..b069c19 --- /dev/null +++ b/Marlin/src/libs/BL24CXX.h @@ -0,0 +1,72 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +/******************************************************************************** + * @file BL24CXX.h + * @brief i2c EEPROM for Ender 3 v2 board (4.2.2) + ********************************************************************************/ + +/******************** IIC ********************/ + +class BL24CXX; + +// All operation functions of IIC +class IIC { +friend class BL24CXX; +protected: + static void init(); // Initialize the IO port of IIC + static void start(); // Send IIC start signal + static void stop(); // Send IIC stop signal + static void send_byte(uint8_t txd); // IIC sends a byte + static uint8_t read_byte(unsigned char ack); // IIC reads a byte + static uint8_t wait_ack(); // IIC waits for ACK signal + static void ack(); // IIC sends ACK signal + static void nAck(); // IIC does not send ACK signal +}; + +/******************** EEPROM ********************/ + +#define BL24C01 127 +#define BL24C02 255 +#define BL24C04 511 +#define BL24C08 1023 +#define BL24C16 2047 +#define BL24C32 4095 +#define BL24C64 8191 +#define BL24C128 16383 +#define BL24C256 32767 +#define EE_TYPE BL24C16 + +class BL24CXX { +private: + static bool _check(); // Check the device +public: + static void init(); // Initialize IIC + static bool check(); // Check / recheck the device + static uint8_t readOneByte(uint16_t ReadAddr); // Read a byte at the specified address + static void writeOneByte(uint16_t WriteAddr, uint8_t DataToWrite); // Write a byte at the specified address + static void writeLenByte(uint16_t WriteAddr, uint32_t DataToWrite, uint8_t Len); // The specified address begins to write the data of the specified length + static uint32_t readLenByte(uint16_t ReadAddr, uint8_t Len); // The specified address starts to read the data of the specified length + static void write(uint16_t WriteAddr, uint8_t *pBuffer, uint16_t NumToWrite); // Write the specified length of data from the specified address + static void read(uint16_t ReadAddr, uint8_t *pBuffer, uint16_t NumToRead); // Read the data of the specified length from the specified address +}; diff --git a/Marlin/src/libs/L64XX/L64XX_Marlin.cpp b/Marlin/src/libs/L64XX/L64XX_Marlin.cpp new file mode 100644 index 0000000..7d36a02 --- /dev/null +++ b/Marlin/src/libs/L64XX/L64XX_Marlin.cpp @@ -0,0 +1,931 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +/** + * The monitor_driver routines are a close copy of the TMC code + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_L64XX + +#include "L64XX_Marlin.h" + +L64XX_Marlin L64xxManager; + +#include "../../module/stepper/indirection.h" +#include "../../gcode/gcode.h" +#include "../../module/planner.h" +#include "../../HAL/shared/Delay.h" + +void echo_yes_no(const bool yes) { serialprintPGM(yes ? PSTR(" YES") : PSTR(" NO ")); } + +static const char str_X[] PROGMEM = "X ", str_Y[] PROGMEM = "Y ", str_Z[] PROGMEM = "Z ", + str_X2[] PROGMEM = "X2", str_Y2[] PROGMEM = "Y2", + str_Z2[] PROGMEM = "Z2", str_Z3[] PROGMEM = "Z3", str_Z4[] PROGMEM = "Z4", + str_E0[] PROGMEM = "E0", str_E1[] PROGMEM = "E1", + str_E2[] PROGMEM = "E2", str_E3[] PROGMEM = "E3", + str_E4[] PROGMEM = "E4", str_E5[] PROGMEM = "E5", + str_E6[] PROGMEM = "E6", str_E7[] PROGMEM = "E7" + ; + +PGM_P const L64XX_Marlin::index_to_axis[] PROGMEM = { + str_X, str_Y, str_Z, str_X2, str_Y2, str_Z2, str_Z3, str_Z4, + str_E0, str_E1, str_E2, str_E3, str_E4, str_E5, str_E6, str_E7 +}; + +#define DEBUG_OUT ENABLED(L6470_CHITCHAT) +#include "../../core/debug_out.h" + +uint8_t L64XX_Marlin::dir_commands[MAX_L64XX]; // array to hold direction command for each driver + +const uint8_t L64XX_Marlin::index_to_dir[MAX_L64XX] = { + INVERT_X_DIR, INVERT_Y_DIR, INVERT_Z_DIR + , (INVERT_X_DIR) ^ BOTH(X_DUAL_STEPPER_DRIVERS, INVERT_X2_VS_X_DIR) // X2 + , (INVERT_Y_DIR) ^ BOTH(Y_DUAL_STEPPER_DRIVERS, INVERT_Y2_VS_Y_DIR) // Y2 + , (INVERT_Z_DIR) ^ ENABLED(INVERT_Z2_VS_Z_DIR) // Z2 + , (INVERT_Z_DIR) ^ ENABLED(INVERT_Z3_VS_Z_DIR) // Z3 + , (INVERT_Z_DIR) ^ ENABLED(INVERT_Z4_VS_Z_DIR) // Z4 + , INVERT_E0_DIR, INVERT_E1_DIR, INVERT_E2_DIR, INVERT_E3_DIR + , INVERT_E4_DIR, INVERT_E5_DIR, INVERT_E6_DIR, INVERT_E7_DIR +}; + +volatile uint8_t L64XX_Marlin::spi_abort = false; +uint8_t L64XX_Marlin::spi_active = false; + +L64XX_Marlin::L64XX_shadow_t L64XX_Marlin::shadow; + +//uint32_t UVLO_ADC = 0x0400; // ADC undervoltage event + +void L6470_populate_chain_array() { + + #define _L6470_INIT_SPI(Q) do{ stepper##Q.set_chain_info(Q, Q##_CHAIN_POS); }while(0) + + #if AXIS_IS_L64XX(X) + _L6470_INIT_SPI(X); + #endif + #if AXIS_IS_L64XX(X2) + _L6470_INIT_SPI(X2); + #endif + #if AXIS_IS_L64XX(Y) + _L6470_INIT_SPI(Y); + #endif + #if AXIS_IS_L64XX(Y2) + _L6470_INIT_SPI(Y2); + #endif + #if AXIS_IS_L64XX(Z) + _L6470_INIT_SPI(Z); + #endif + #if AXIS_IS_L64XX(Z2) + _L6470_INIT_SPI(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + _L6470_INIT_SPI(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + _L6470_INIT_SPI(Z4); + #endif + #if AXIS_IS_L64XX(E0) + _L6470_INIT_SPI(E0); + #endif + #if AXIS_IS_L64XX(E1) + _L6470_INIT_SPI(E1); + #endif + #if AXIS_IS_L64XX(E2) + _L6470_INIT_SPI(E2); + #endif + #if AXIS_IS_L64XX(E3) + _L6470_INIT_SPI(E3); + #endif + #if AXIS_IS_L64XX(E4) + _L6470_INIT_SPI(E4); + #endif + #if AXIS_IS_L64XX(E5) + _L6470_INIT_SPI(E5); + #endif + #if AXIS_IS_L64XX(E6) + _L6470_INIT_SPI(E6); + #endif + #if AXIS_IS_L64XX(E7) + _L6470_INIT_SPI(E7); + #endif +} + + +/** + * Some status bit positions & definitions differ per driver. + * Copy info to known locations to simplfy check/display logic. + * 1. Copy stepper status + * 2. Copy status bit definitions + * 3. Copy status layout + * 4. Make all error bits active low (as needed) + */ +uint16_t L64XX_Marlin::get_stepper_status(L64XX &st) { + shadow.STATUS_AXIS_RAW = st.getStatus(); + shadow.STATUS_AXIS = shadow.STATUS_AXIS_RAW; + shadow.STATUS_AXIS_LAYOUT = st.L6470_status_layout; + shadow.AXIS_OCD_TH_MAX = st.OCD_TH_MAX; + shadow.AXIS_STALL_TH_MAX = st.STALL_TH_MAX; + shadow.AXIS_OCD_CURRENT_CONSTANT_INV = st.OCD_CURRENT_CONSTANT_INV; + shadow.AXIS_STALL_CURRENT_CONSTANT_INV = st.STALL_CURRENT_CONSTANT_INV; + shadow.L6470_AXIS_CONFIG = st.L64XX_CONFIG; + shadow.L6470_AXIS_STATUS = st.L64XX_STATUS; + shadow.STATUS_AXIS_OCD = st.STATUS_OCD; + shadow.STATUS_AXIS_SCK_MOD = st.STATUS_SCK_MOD; + shadow.STATUS_AXIS_STEP_LOSS_A = st.STATUS_STEP_LOSS_A; + shadow.STATUS_AXIS_STEP_LOSS_B = st.STATUS_STEP_LOSS_B; + shadow.STATUS_AXIS_TH_SD = st.STATUS_TH_SD; + shadow.STATUS_AXIS_TH_WRN = st.STATUS_TH_WRN; + shadow.STATUS_AXIS_UVLO = st.STATUS_UVLO; + shadow.STATUS_AXIS_WRONG_CMD = st.STATUS_WRONG_CMD; + shadow.STATUS_AXIS_CMD_ERR = st.STATUS_CMD_ERR; + shadow.STATUS_AXIS_NOTPERF_CMD = st.STATUS_NOTPERF_CMD; + + switch (shadow.STATUS_AXIS_LAYOUT) { + case L6470_STATUS_LAYOUT: { // L6470 + shadow.L6470_ERROR_MASK = shadow.STATUS_AXIS_UVLO | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD | shadow.STATUS_AXIS_OCD | shadow.STATUS_AXIS_STEP_LOSS_A | shadow.STATUS_AXIS_STEP_LOSS_B; + shadow.STATUS_AXIS ^= (shadow.STATUS_AXIS_WRONG_CMD | shadow.STATUS_AXIS_NOTPERF_CMD); // invert just error bits that are active high + break; + } + case L6474_STATUS_LAYOUT: { // L6474 + shadow.L6470_ERROR_MASK = shadow.STATUS_AXIS_UVLO | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD | shadow.STATUS_AXIS_OCD ; + shadow.STATUS_AXIS ^= (shadow.STATUS_AXIS_WRONG_CMD | shadow.STATUS_AXIS_NOTPERF_CMD); // invert just error bits that are active high + break; + } + case L6480_STATUS_LAYOUT: { // L6480 & powerSTEP01 + shadow.L6470_ERROR_MASK = shadow.STATUS_AXIS_UVLO | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD | shadow.STATUS_AXIS_OCD | shadow.STATUS_AXIS_STEP_LOSS_A | shadow.STATUS_AXIS_STEP_LOSS_B; + shadow.STATUS_AXIS ^= (shadow.STATUS_AXIS_CMD_ERR | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD); // invert just error bits that are active high + break; + } + } + return shadow.STATUS_AXIS; +} + + +void L64XX_Marlin::init() { // Set up SPI and then init chips + ENABLE_RESET_L64XX_CHIPS(LOW); // hardware reset of drivers + DELAY_US(100); + ENABLE_RESET_L64XX_CHIPS(HIGH); + DELAY_US(1000); // need about 650µs for the chip(s) to fully start up + L6470_populate_chain_array(); // Set up array to control where in the SPI transfer sequence a particular stepper's data goes + + spi_init(); // Since L64XX SPI pins are unset we must init SPI here + + init_to_defaults(); // init the chips +} + +uint16_t L64XX_Marlin::get_status(const L64XX_axis_t axis) { + + #define STATUS_L6470(Q) get_stepper_status(stepper##Q) + + switch (axis) { + default: break; + #if AXIS_IS_L64XX(X) + case X : return STATUS_L6470(X); + #endif + #if AXIS_IS_L64XX(Y) + case Y : return STATUS_L6470(Y); + #endif + #if AXIS_IS_L64XX(Z) + case Z : return STATUS_L6470(Z); + #endif + #if AXIS_IS_L64XX(X2) + case X2: return STATUS_L6470(X2); + #endif + #if AXIS_IS_L64XX(Y2) + case Y2: return STATUS_L6470(Y2); + #endif + #if AXIS_IS_L64XX(Z2) + case Z2: return STATUS_L6470(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + case Z3: return STATUS_L6470(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + case Z4: return STATUS_L6470(Z4); + #endif + #if AXIS_IS_L64XX(E0) + case E0: return STATUS_L6470(E0); + #endif + #if AXIS_IS_L64XX(E1) + case E1: return STATUS_L6470(E1); + #endif + #if AXIS_IS_L64XX(E2) + case E2: return STATUS_L6470(E2); + #endif + #if AXIS_IS_L64XX(E3) + case E3: return STATUS_L6470(E3); + #endif + #if AXIS_IS_L64XX(E4) + case E4: return STATUS_L6470(E4); + #endif + #if AXIS_IS_L64XX(E5) + case E5: return STATUS_L6470(E5); + #endif + #if AXIS_IS_L64XX(E6) + case E6: return STATUS_L6470(E6); + #endif + #if AXIS_IS_L64XX(E7) + case E7: return STATUS_L6470(E7); + #endif + } + + return 0; // Not needed but kills a compiler warning +} + +uint32_t L64XX_Marlin::get_param(const L64XX_axis_t axis, const uint8_t param) { + + #define GET_L6470_PARAM(Q) L6470_GETPARAM(param, Q) + + switch (axis) { + default: break; + #if AXIS_IS_L64XX(X) + case X : return GET_L6470_PARAM(X); + #endif + #if AXIS_IS_L64XX(Y) + case Y : return GET_L6470_PARAM(Y); + #endif + #if AXIS_IS_L64XX(Z) + case Z : return GET_L6470_PARAM(Z); + #endif + #if AXIS_IS_L64XX(X2) + case X2: return GET_L6470_PARAM(X2); + #endif + #if AXIS_IS_L64XX(Y2) + case Y2: return GET_L6470_PARAM(Y2); + #endif + #if AXIS_IS_L64XX(Z2) + case Z2: return GET_L6470_PARAM(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + case Z3: return GET_L6470_PARAM(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + case Z4: return GET_L6470_PARAM(Z4); + #endif + #if AXIS_IS_L64XX(E0) + case E0: return GET_L6470_PARAM(E0); + #endif + #if AXIS_IS_L64XX(E1) + case E1: return GET_L6470_PARAM(E1); + #endif + #if AXIS_IS_L64XX(E2) + case E2: return GET_L6470_PARAM(E2); + #endif + #if AXIS_IS_L64XX(E3) + case E3: return GET_L6470_PARAM(E3); + #endif + #if AXIS_IS_L64XX(E4) + case E4: return GET_L6470_PARAM(E4); + #endif + #if AXIS_IS_L64XX(E5) + case E5: return GET_L6470_PARAM(E5); + #endif + #if AXIS_IS_L64XX(E6) + case E6: return GET_L6470_PARAM(E6); + #endif + #if AXIS_IS_L64XX(E7) + case E7: return GET_L6470_PARAM(E7); + #endif + } + + return 0; // not needed but kills a compiler warning +} + +void L64XX_Marlin::set_param(const L64XX_axis_t axis, const uint8_t param, const uint32_t value) { + + #define SET_L6470_PARAM(Q) stepper##Q.SetParam(param, value) + + switch (axis) { + default: break; + #if AXIS_IS_L64XX(X) + case X : SET_L6470_PARAM(X); break; + #endif + #if AXIS_IS_L64XX(Y) + case Y : SET_L6470_PARAM(Y); break; + #endif + #if AXIS_IS_L64XX(Z) + case Z : SET_L6470_PARAM(Z); break; + #endif + #if AXIS_IS_L64XX(X2) + case X2: SET_L6470_PARAM(X2); break; + #endif + #if AXIS_IS_L64XX(Y2) + case Y2: SET_L6470_PARAM(Y2); break; + #endif + #if AXIS_IS_L64XX(Z2) + case Z2: SET_L6470_PARAM(Z2); break; + #endif + #if AXIS_IS_L64XX(Z3) + case Z3: SET_L6470_PARAM(Z3); break; + #endif + #if AXIS_IS_L64XX(Z4) + case Z4: SET_L6470_PARAM(Z4); break; + #endif + #if AXIS_IS_L64XX(E0) + case E0: SET_L6470_PARAM(E0); break; + #endif + #if AXIS_IS_L64XX(E1) + case E1: SET_L6470_PARAM(E1); break; + #endif + #if AXIS_IS_L64XX(E2) + case E2: SET_L6470_PARAM(E2); break; + #endif + #if AXIS_IS_L64XX(E3) + case E3: SET_L6470_PARAM(E3); break; + #endif + #if AXIS_IS_L64XX(E4) + case E4: SET_L6470_PARAM(E4); break; + #endif + #if AXIS_IS_L64XX(E5) + case E5: SET_L6470_PARAM(E5); break; + #endif + #if AXIS_IS_L64XX(E6) + case E6: SET_L6470_PARAM(E6); break; + #endif + #if AXIS_IS_L64XX(E7) + case E7: SET_L6470_PARAM(E7); break; + #endif + } +} + +inline void echo_min_max(const char a, const float &min, const float &max) { + DEBUG_CHAR(' '); DEBUG_CHAR(a); + DEBUG_ECHOPAIR(" min = ", min); + DEBUG_ECHOLNPAIR(" max = ", max); +} +inline void echo_oct_used(const float &oct, const uint8_t stall) { + DEBUG_ECHOPAIR("over_current_threshold used : ", oct); + serialprintPGM(stall ? PSTR(" (Stall") : PSTR(" (OCD")); + DEBUG_ECHOLNPGM(" threshold)"); +} +inline void err_out_of_bounds() { DEBUG_ECHOLNPGM("Test aborted - motion out of bounds"); } + +uint8_t L64XX_Marlin::get_user_input(uint8_t &driver_count, L64XX_axis_t axis_index[3], char axis_mon[3][3], + float &position_max, float &position_min, float &final_feedrate, uint8_t &kval_hold, + uint8_t over_current_flag, uint8_t &OCD_TH_val, uint8_t &STALL_TH_val, uint16_t &over_current_threshold +) { + // Return TRUE if the calling routine needs to abort/kill + + uint16_t displacement = 0; // " = 0" to eliminate compiler warning + uint8_t j; // general purpose counter + + if (!all_axes_homed()) { + DEBUG_ECHOLNPGM("Test aborted - home all before running this command"); + return true; + } + + uint8_t found_displacement = false; + LOOP_XYZE(i) if (uint16_t _displacement = parser.intval(axis_codes[i])) { + found_displacement = true; + displacement = _displacement; + uint8_t axis_offset = parser.byteval('J'); + axis_mon[0][0] = axis_codes[i]; // Axis first character, one of XYZE + const bool single_or_e = axis_offset >= 2 || axis_mon[0][0] == 'E', + one_or_more = !single_or_e && axis_offset == 0; + uint8_t driver_count_local = 0; // Can't use "driver_count" directly as a subscript because it's passed by reference + if (single_or_e) // Single axis, E0, or E1 + axis_mon[0][1] = axis_offset + '0'; // Index given by 'J' parameter + + if (single_or_e || one_or_more) { + for (j = 0; j < MAX_L64XX; j++) { // Count up the drivers on this axis + PGM_P str = (PGM_P)pgm_read_ptr(&index_to_axis[j]); // Get a PGM_P from progmem + const char c = pgm_read_byte(str); // Get a char from progmem + if (axis_mon[0][0] == c) { // For each stepper on this axis... + char *mon = axis_mon[driver_count_local]; + *mon++ = c; // Copy the 3 letter axis name + *mon++ = pgm_read_byte(&str[1]); // to the axis_mon array + *mon = pgm_read_byte(&str[2]); + axis_index[driver_count_local] = (L64XX_axis_t)j; // And store the L64XX axis index + driver_count_local++; + } + } + if (one_or_more) driver_count = driver_count_local; + } + break; // only take first axis found + } + + if (!found_displacement) { + DEBUG_ECHOLNPGM("Test aborted - AXIS with displacement is required"); + return true; + } + + // + // Position calcs & checks + // + + const float X_center = LOGICAL_X_POSITION(current_position.x), + Y_center = LOGICAL_Y_POSITION(current_position.y), + Z_center = LOGICAL_Z_POSITION(current_position.z), + E_center = current_position.e; + + switch (axis_mon[0][0]) { + default: position_max = position_min = 0; break; + + case 'X': { + position_min = X_center - displacement; + position_max = X_center + displacement; + echo_min_max('X', position_min, position_max); + if (false + #ifdef X_MIN_POS + || position_min < (X_MIN_POS) + #endif + #ifdef X_MAX_POS + || position_max > (X_MAX_POS) + #endif + ) { + err_out_of_bounds(); + return true; + } + } break; + + case 'Y': { + position_min = Y_center - displacement; + position_max = Y_center + displacement; + echo_min_max('Y', position_min, position_max); + if (false + #ifdef Y_MIN_POS + || position_min < (Y_MIN_POS) + #endif + #ifdef Y_MAX_POS + || position_max > (Y_MAX_POS) + #endif + ) { + err_out_of_bounds(); + return true; + } + } break; + + case 'Z': { + position_min = Z_center - displacement; + position_max = Z_center + displacement; + echo_min_max('Z', position_min, position_max); + if (false + #ifdef Z_MIN_POS + || position_min < (Z_MIN_POS) + #endif + #ifdef Z_MAX_POS + || position_max > (Z_MAX_POS) + #endif + ) { + err_out_of_bounds(); + return true; + } + } break; + + case 'E': { + position_min = E_center - displacement; + position_max = E_center + displacement; + echo_min_max('E', position_min, position_max); + } break; + } + + // + // Work on the drivers + // + + LOOP_L_N(k, driver_count) { + uint8_t not_found = true; + for (j = 1; j <= L64XX::chain[0]; j++) { + PGM_P const str = (PGM_P)pgm_read_ptr(&index_to_axis[L64XX::chain[j]]); + if (pgm_read_byte(&str[0]) == axis_mon[k][0] && pgm_read_byte(&str[1]) == axis_mon[k][1]) { // See if a L6470 driver + not_found = false; + break; + } + } + if (not_found) { + driver_count = k; + axis_mon[k][0] = ' '; // mark this entry invalid + break; + } + } + + if (driver_count == 0) { + DEBUG_ECHOLNPGM("Test aborted - not a L6470 axis"); + return true; + } + + DEBUG_ECHOPGM("Monitoring:"); + for (j = 0; j < driver_count; j++) DEBUG_ECHOPAIR(" ", axis_mon[j]); + DEBUG_EOL(); + + // now have a list of driver(s) to monitor + + // + // TVAL & kVAL_HOLD checks & settings + // + const L64XX_shadow_t &sh = shadow; + get_status(axis_index[0]); // populate shadow array + + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // L6474 - use TVAL + uint16_t TVAL_current = parser.ushortval('T'); + if (TVAL_current) { + uint8_t TVAL_count = (TVAL_current / sh.AXIS_STALL_CURRENT_CONSTANT_INV) - 1; + LIMIT(TVAL_count, 0, sh.AXIS_STALL_TH_MAX); + for (j = 0; j < driver_count; j++) + set_param(axis_index[j], L6474_TVAL, TVAL_count); + } + // only print the tval from one of the drivers + kval_hold = get_param(axis_index[0], L6474_TVAL); + DEBUG_ECHOLNPAIR("TVAL current (mA) = ", (kval_hold + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV); + } + else { + kval_hold = parser.byteval('K'); + if (kval_hold) { + DEBUG_ECHOLNPAIR("kval_hold = ", kval_hold); + for (j = 0; j < driver_count; j++) + set_param(axis_index[j], L6470_KVAL_HOLD, kval_hold); + } + else { + // only print the KVAL_HOLD from one of the drivers + kval_hold = get_param(axis_index[0], L6470_KVAL_HOLD); + DEBUG_ECHOLNPAIR("KVAL_HOLD = ", kval_hold); + } + } + + // + // Overcurrent checks & settings + // + + if (over_current_flag) { + + uint8_t OCD_TH_val_local = 0, // compiler thinks OCD_TH_val is unused if use it directly + STALL_TH_val_local = 0; // just in case ... + + over_current_threshold = parser.intval('I'); + + if (over_current_threshold) { + + OCD_TH_val_local = over_current_threshold/375; + LIMIT(OCD_TH_val_local, 0, 15); + STALL_TH_val_local = over_current_threshold/31.25; + LIMIT(STALL_TH_val_local, 0, 127); + uint16_t OCD_TH_actual = (OCD_TH_val_local + 1) * 375, + STALL_TH_actual = (STALL_TH_val_local + 1) * 31.25; + if (OCD_TH_actual < STALL_TH_actual) { + OCD_TH_val_local++; + OCD_TH_actual = (OCD_TH_val_local + 1) * 375; + } + + DEBUG_ECHOLNPAIR("over_current_threshold specified: ", over_current_threshold); + if (!(sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)) echo_oct_used((STALL_TH_val_local + 1) * 31.25, true); + echo_oct_used((OCD_TH_val_local + 1) * 375, false); + + #define SET_OVER_CURRENT(Q) do { stepper##Q.SetParam(L6470_STALL_TH, STALL_TH_val_local); stepper##Q.SetParam(L6470_OCD_TH, OCD_TH_val_local);} while (0) + + for (j = 0; j < driver_count; j++) { + set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val_local); + set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val_local); + } + } + else { + // only get & print the OVER_CURRENT values from one of the drivers + STALL_TH_val_local = get_param(axis_index[0], L6470_STALL_TH); + OCD_TH_val_local = get_param(axis_index[0], L6470_OCD_TH); + + if (!(sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)) echo_oct_used((STALL_TH_val_local + 1) * 31.25, true); + echo_oct_used((OCD_TH_val_local + 1) * 375, false); + } // over_current_threshold + + for (j = 0; j < driver_count; j++) { // set all drivers on axis the same + set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val_local); + set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val_local); + } + + OCD_TH_val = OCD_TH_val_local; // force compiler to update the main routine's copy + STALL_TH_val = STALL_TH_val_local; // force compiler to update the main routine's copy + } // end of overcurrent + + // + // Feedrate + // + + final_feedrate = parser.floatval('F'); + if (final_feedrate == 0) { + static constexpr float default_max_feedrate[] = DEFAULT_MAX_FEEDRATE; + const uint8_t num_feedrates = COUNT(default_max_feedrate); + for (j = 0; j < num_feedrates; j++) { + if (axis_codes[j] == axis_mon[0][0]) { + final_feedrate = default_max_feedrate[j]; + break; + } + } + if (j == 3 && num_feedrates > 4) { // have more than one extruder feedrate + uint8_t extruder_num = axis_mon[0][1] - '0'; + if (j <= num_feedrates - extruder_num) // have a feedrate specifically for this extruder + final_feedrate = default_max_feedrate[j + extruder_num]; + else + final_feedrate = default_max_feedrate[3]; // use E0 feedrate for this extruder + } + final_feedrate *= 60; // convert to mm/minute + } // end of feedrate + + return false; // FALSE indicates no user input problems +} + +void L64XX_Marlin::say_axis(const L64XX_axis_t axis, const uint8_t label/*=true*/) { + if (label) SERIAL_ECHOPGM("AXIS:"); + const char * const str = L64xxManager.index_to_axis[axis]; + SERIAL_CHAR(' ', str[0], str[1], ' '); +} + +#if ENABLED(L6470_CHITCHAT) + + // Assumes status bits have been inverted + void L64XX_Marlin::error_status_decode(const uint16_t status, const L64XX_axis_t axis, + const uint16_t _status_axis_th_sd, const uint16_t _status_axis_th_wrn, + const uint16_t _status_axis_step_loss_a, const uint16_t _status_axis_step_loss_b, + const uint16_t _status_axis_ocd, const uint8_t _status_axis_layout + ) { + say_axis(axis); + DEBUG_ECHOPGM(" THERMAL: "); + serialprintPGM((status & _status_axis_th_sd) ? PSTR("SHUTDOWN") : (status & _status_axis_th_wrn) ? PSTR("WARNING ") : PSTR("OK ")); + DEBUG_ECHOPGM(" OVERCURRENT: "); + echo_yes_no((status & _status_axis_ocd) != 0); + if (!(_status_axis_layout == L6474_STATUS_LAYOUT)) { // L6474 doesn't have these bits + DEBUG_ECHOPGM(" STALL: "); + echo_yes_no((status & (_status_axis_step_loss_a | _status_axis_step_loss_b)) != 0); + } + DEBUG_EOL(); + } + +#endif + +////////////////////////////////////////////////////////////////////////////////////////////////// +//// +//// MONITOR_L6470_DRIVER_STATUS routines +//// +////////////////////////////////////////////////////////////////////////////////////////////////// + +#if ENABLED(MONITOR_L6470_DRIVER_STATUS) + + bool L64XX_Marlin::monitor_paused = false; // Flag to skip monitor during M122, M906, M916, M917, M918, etc. + + struct L6470_driver_data { + uint8_t driver_index; + uint32_t driver_status; + uint8_t is_otw; + uint8_t otw_counter; + uint8_t is_ot; + uint8_t is_hi_Z; + uint8_t com_counter; + }; + + L6470_driver_data driver_L6470_data[] = { + #if AXIS_IS_L64XX(X) + { 0, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(Y) + { 1, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(Z) + { 2, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(X2) + { 3, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(Y2) + { 4, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(Z2) + { 5, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(Z3) + { 6, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(Z4) + { 7, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(E0) + { 8, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(E1) + { 9, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(E2) + { 10, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(E3) + { 11, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(E4) + { 12, 0, 0, 0, 0, 0, 0 }, + #endif + #if AXIS_IS_L64XX(E5) + { 13, 0, 0, 0, 0, 0, 0 } + #endif + #if AXIS_IS_L64XX(E6) + { 14, 0, 0, 0, 0, 0, 0 } + #endif + #if AXIS_IS_L64XX(E7) + { 16, 0, 0, 0, 0, 0, 0 } + #endif + }; + + void L64XX_Marlin::append_stepper_err(char* &p, const uint8_t stepper_index, const char * const err/*=nullptr*/) { + PGM_P const str = (PGM_P)pgm_read_ptr(&index_to_axis[stepper_index]); + p += sprintf_P(p, PSTR("Stepper %c%c "), pgm_read_byte(&str[0]), pgm_read_byte(&str[1])); + if (err) p += sprintf_P(p, err); + } + + void L64XX_Marlin::monitor_update(L64XX_axis_t stepper_index) { + if (spi_abort) return; // don't do anything if set_directions() has occurred + const L64XX_shadow_t &sh = shadow; + get_status(stepper_index); // get stepper status and details + uint16_t status = sh.STATUS_AXIS; + uint8_t kval_hold, tval; + char temp_buf[120], *p = temp_buf; + uint8_t j; + for (j = 0; j < L64XX::chain[0]; j++) // find the table for this stepper + if (driver_L6470_data[j].driver_index == stepper_index) break; + + driver_L6470_data[j].driver_status = status; + uint16_t _status = ~status; // all error bits are active low + + if (status == 0 || status == 0xFFFF) { // com problem + if (driver_L6470_data[j].com_counter == 0) { // warn user when it first happens + driver_L6470_data[j].com_counter++; + append_stepper_err(p, stepper_index, PSTR(" - communications lost\n")); + DEBUG_ECHO(temp_buf); + } + else { + driver_L6470_data[j].com_counter++; + if (driver_L6470_data[j].com_counter > 240) { // remind of com problem about every 2 minutes + driver_L6470_data[j].com_counter = 1; + append_stepper_err(p, stepper_index, PSTR(" - still no communications\n")); + DEBUG_ECHO(temp_buf); + } + } + } + else { + if (driver_L6470_data[j].com_counter) { // comms re-established + driver_L6470_data[j].com_counter = 0; + append_stepper_err(p, stepper_index, PSTR(" - communications re-established\n.. setting all drivers to default values\n")); + DEBUG_ECHO(temp_buf); + init_to_defaults(); + } + else { + // no com problems - do the usual checks + if (_status & sh.L6470_ERROR_MASK) { + append_stepper_err(p, stepper_index); + + if (status & STATUS_HIZ) { // The driver has shut down. HiZ is active high + driver_L6470_data[j].is_hi_Z = true; + p += sprintf_P(p, PSTR("%cIS SHUT DOWN"), ' '); + //if (_status & sh.STATUS_AXIS_TH_SD) { // strange - TH_SD never seems to go active, must be implied by the HiZ and TH_WRN + if (_status & sh.STATUS_AXIS_TH_WRN) { // over current shutdown + p += sprintf_P(p, PSTR("%cdue to over temperature"), ' '); + driver_L6470_data[j].is_ot = true; + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // L6474 + tval = get_param(stepper_index, L6474_TVAL) - 2 * KVAL_HOLD_STEP_DOWN; + set_param(stepper_index, L6474_TVAL, tval); // reduce TVAL + p += sprintf_P(p, PSTR(" - TVAL reduced by %d to %d mA"), uint16_t (2 * KVAL_HOLD_STEP_DOWN * sh.AXIS_STALL_CURRENT_CONSTANT_INV), uint16_t ((tval + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV)); // let user know + } + else { + kval_hold = get_param(stepper_index, L6470_KVAL_HOLD) - 2 * KVAL_HOLD_STEP_DOWN; + set_param(stepper_index, L6470_KVAL_HOLD, kval_hold); // reduce KVAL_HOLD + p += sprintf_P(p, PSTR(" - KVAL_HOLD reduced by %d to %d"), 2 * KVAL_HOLD_STEP_DOWN, kval_hold); // let user know + } + } + else + driver_L6470_data[j].is_ot = false; + } + else { + driver_L6470_data[j].is_hi_Z = false; + + if (_status & sh.STATUS_AXIS_TH_WRN) { // have an over temperature warning + driver_L6470_data[j].is_otw = true; + driver_L6470_data[j].otw_counter++; + kval_hold = get_param(stepper_index, L6470_KVAL_HOLD); + if (driver_L6470_data[j].otw_counter > 4) { // otw present for 2 - 2.5 seconds, reduce KVAL_HOLD + driver_L6470_data[j].otw_counter = 0; + driver_L6470_data[j].is_otw = true; + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // L6474 + tval = get_param(stepper_index, L6474_TVAL) - KVAL_HOLD_STEP_DOWN; + set_param(stepper_index, L6474_TVAL, tval); // reduce TVAL + p += sprintf_P(p, PSTR(" - TVAL reduced by %d to %d mA"), uint16_t (KVAL_HOLD_STEP_DOWN * sh.AXIS_STALL_CURRENT_CONSTANT_INV), uint16_t ((tval + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV)); // let user know + } + else { + kval_hold = get_param(stepper_index, L6470_KVAL_HOLD) - KVAL_HOLD_STEP_DOWN; + set_param(stepper_index, L6470_KVAL_HOLD, kval_hold); // reduce KVAL_HOLD + p += sprintf_P(p, PSTR(" - KVAL_HOLD reduced by %d to %d"), KVAL_HOLD_STEP_DOWN, kval_hold); // let user know + } + } + else if (driver_L6470_data[j].otw_counter) + p += sprintf_P(p, PSTR("%c- thermal warning"), ' '); // warn user + } + } + + #if ENABLED(L6470_STOP_ON_ERROR) + if (_status & (sh.STATUS_AXIS_UVLO | sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) + kill(temp_buf); + #endif + + #if ENABLED(L6470_CHITCHAT) + if (_status & sh.STATUS_AXIS_OCD) + p += sprintf_P(p, PSTR("%c over current"), ' '); + + if (_status & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) + p += sprintf_P(p, PSTR("%c stall"), ' '); + + if (_status & sh.STATUS_AXIS_UVLO) + p += sprintf_P(p, PSTR("%c under voltage lock out"), ' '); + + p += sprintf_P(p, PSTR("%c\n"), ' '); + #endif + + DEBUG_ECHOLN(temp_buf); // print the error message + } + else { + driver_L6470_data[j].is_ot = false; + driver_L6470_data[j].otw_counter = 0; //clear out warning indicators + driver_L6470_data[j].is_otw = false; + } // end usual checks + + } // comms established but have errors + } // comms re-established + } // end monitor_update() + + + void L64XX_Marlin::monitor_driver() { + static millis_t next_cOT = 0; + if (ELAPSED(millis(), next_cOT)) { + next_cOT = millis() + 500; + + if (!monitor_paused) { // Skip during M122, M906, M916, M917 or M918 (could steal status result from test) + + spi_active = true; // Tell set_directions() a series of SPI transfers is underway + + #if AXIS_IS_L64XX(X) + monitor_update(X); + #endif + #if AXIS_IS_L64XX(Y) + monitor_update(Y); + #endif + #if AXIS_IS_L64XX(Z) + monitor_update(Z); + #endif + #if AXIS_IS_L64XX(X2) + monitor_update(X2); + #endif + #if AXIS_IS_L64XX(Y2) + monitor_update(Y2); + #endif + #if AXIS_IS_L64XX(Z2) + monitor_update(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + monitor_update(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + monitor_update(Z4); + #endif + #if AXIS_IS_L64XX(E0) + monitor_update(E0); + #endif + #if AXIS_IS_L64XX(E1) + monitor_update(E1); + #endif + #if AXIS_IS_L64XX(E2) + monitor_update(E2); + #endif + #if AXIS_IS_L64XX(E3) + monitor_update(E3); + #endif + #if AXIS_IS_L64XX(E4) + monitor_update(E4); + #endif + #if AXIS_IS_L64XX(E5) + monitor_update(E5); + #endif + + if (TERN0(L6470_DEBUG, report_L6470_status)) DEBUG_EOL(); + + spi_active = false; // done with all SPI transfers - clear handshake flags + spi_abort = false; + } + } + } + +#endif // MONITOR_L6470_DRIVER_STATUS + +#endif // HAS_L64XX diff --git a/Marlin/src/libs/L64XX/L64XX_Marlin.h b/Marlin/src/libs/L64XX/L64XX_Marlin.h new file mode 100644 index 0000000..c8d2739 --- /dev/null +++ b/Marlin/src/libs/L64XX/L64XX_Marlin.h @@ -0,0 +1,139 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../../inc/MarlinConfig.h" + +#include <L6470.h> +#if !(L6470_LIBRARY_VERSION >= 0x000800) + #error 'L6470_LIBRARY_VERSION 0x000800 or later required' +#endif + +#define L6470_GETPARAM(P,Q) stepper##Q.GetParam(P) + +#define dSPIN_STEP_CLOCK 0x58 +#define dSPIN_STEP_CLOCK_FWD dSPIN_STEP_CLOCK +#define dSPIN_STEP_CLOCK_REV dSPIN_STEP_CLOCK+1 +#define HAS_L64XX_EXTRUDER (AXIS_IS_L64XX(E0) || AXIS_IS_L64XX(E1) || AXIS_IS_L64XX(E2) || AXIS_IS_L64XX(E3) || AXIS_IS_L64XX(E4) || AXIS_IS_L64XX(E5) || AXIS_IS_L64XX(E6) || AXIS_IS_L64XX(E7)) + +enum L64XX_axis_t : uint8_t { X, Y, Z, X2, Y2, Z2, Z3, Z4, E0, E1, E2, E3, E4, E5, E6, E7, MAX_L64XX }; + +class L64XX_Marlin : public L64XXHelper { +public: + static PGM_P const index_to_axis[MAX_L64XX]; + + static const uint8_t index_to_dir[MAX_L64XX]; + + static uint8_t dir_commands[MAX_L64XX]; + + // Flags to guarantee graceful switch if stepper interrupts L6470 SPI transfer + static volatile uint8_t spi_abort; + static uint8_t spi_active; + + L64XX_Marlin() {} + + static void init(); + static void init_to_defaults(); + + static uint16_t get_stepper_status(L64XX &st); + + static uint16_t get_status(const L64XX_axis_t axis); + + static uint32_t get_param(const L64XX_axis_t axis, const uint8_t param); + + static void set_param(const L64XX_axis_t axis, const uint8_t param, const uint32_t value); + + //static void send_command(const L64XX_axis_t axis, uint8_t command); + + static uint8_t get_user_input(uint8_t &driver_count, L64XX_axis_t axis_index[3], char axis_mon[3][3], + float &position_max, float &position_min, float &final_feedrate, uint8_t &kval_hold, + uint8_t over_current_flag, uint8_t &OCD_TH_val, uint8_t &STALL_TH_val, uint16_t &over_current_threshold); + + static void transfer(uint8_t L6470_buf[], const uint8_t length); + + static void say_axis(const L64XX_axis_t axis, const uint8_t label=true); + #if ENABLED(L6470_CHITCHAT) + static void error_status_decode( + const uint16_t status, const L64XX_axis_t axis, + const uint16_t _status_axis_th_sd, const uint16_t _status_axis_th_wrn, + const uint16_t _status_axis_step_loss_a, const uint16_t _status_axis_step_loss_b, + const uint16_t _status_axis_ocd, const uint8_t _status_axis_layout + ); + #else + FORCE_INLINE static void error_status_decode( + const uint16_t, const L64XX_axis_t, + const uint16_t, const uint16_t, + const uint16_t, const uint16_t, + const uint16_t, const uint8_t + ){} + #endif + + // ~40 bytes SRAM to simplify status decode routines + typedef struct { + uint8_t STATUS_AXIS_LAYOUT; // Copy of L6470_status_layout + uint8_t AXIS_OCD_TH_MAX; // Size of OCD_TH field + uint8_t AXIS_STALL_TH_MAX; // Size of STALL_TH field + float AXIS_OCD_CURRENT_CONSTANT_INV; // mA per count + float AXIS_STALL_CURRENT_CONSTANT_INV; // mA per count + uint8_t L6470_AXIS_CONFIG, // Address of the CONFIG register + L6470_AXIS_STATUS; // Address of the STATUS register + uint16_t L6470_ERROR_MASK, // STATUS_UVLO | STATUS_TH_WRN | STATUS_TH_SD | STATUS_OCD | STATUS_STEP_LOSS_A | STATUS_STEP_LOSS_B + L6474_ERROR_MASK, // STATUS_UVLO | STATUS_TH_WRN | STATUS_TH_SD | STATUS_OCD + STATUS_AXIS_RAW, // Copy of status register contents + STATUS_AXIS, // Copy of status register contents but with all error bits active low + STATUS_AXIS_OCD, // Overcurrent detected bit position + STATUS_AXIS_SCK_MOD, // Step clock mode is active bit position + STATUS_AXIS_STEP_LOSS_A, // Stall detected on A bridge bit position + STATUS_AXIS_STEP_LOSS_B, // Stall detected on B bridge bit position + STATUS_AXIS_TH_SD, // Thermal shutdown bit position + STATUS_AXIS_TH_WRN, // Thermal warning bit position + STATUS_AXIS_UVLO, // Undervoltage lockout is active bit position + STATUS_AXIS_WRONG_CMD, // Last command not valid bit position + STATUS_AXIS_CMD_ERR, // Command error bit position + STATUS_AXIS_NOTPERF_CMD; // Last command not performed bit position + } L64XX_shadow_t; + + static L64XX_shadow_t shadow; + + #if ENABLED(MONITOR_L6470_DRIVER_STATUS) + static bool monitor_paused; + static inline void pause_monitor(const bool p) { monitor_paused = p; } + static void monitor_update(L64XX_axis_t stepper_index); + static void monitor_driver(); + #else + static inline void pause_monitor(const bool) {} + #endif + +//protected: + // L64XXHelper methods + static void spi_init(); + static uint8_t transfer_single(uint8_t data, int16_t ss_pin); + static uint8_t transfer_chain(uint8_t data, int16_t ss_pin, uint8_t chain_position); + +private: + static void append_stepper_err(char* &p, const uint8_t stepper_index, const char * const err=nullptr); + +}; + +void echo_yes_no(const bool yes); + +extern L64XX_Marlin L64xxManager; diff --git a/Marlin/src/libs/L64XX/README.md b/Marlin/src/libs/L64XX/README.md new file mode 100644 index 0000000..d28bec5 --- /dev/null +++ b/Marlin/src/libs/L64XX/README.md @@ -0,0 +1,98 @@ +### L64XX Stepper Driver + +*Arduino-L6470* library revision 0.8.0 or above is required. + +This software can be used with the L6470, L6474, L6480 and the powerSTEP01 (collectively referred to as "L64xx" from now on). Different drivers can be mixed within a system. + +These devices use voltage PWMs to drive the stepper phases. On the L6474 the phase current is controlled by the `TVAL` register. On all the other drivers the phase current is indirectly controlled via the `KVAL_HOLD` register which scales the PWM duty cycle. + +This software assumes that all drivers are in one SPI daisy chain. + +### Hardware Setup + +- MOSI from controller tied to SDI on the first device + +- SDO of the first device is tied to SDI of the next device + +- SDO of the last device is tied to MISO of the controller + +- All devices share the same `SCK_PIN` and `SS_PIN` pins. The user must supply a macro to control the `RESET_PIN`(s). + +- Each L6470 passes the data it saw on its SDI to its neighbor on the **NEXT** SPI cycle (8 bit delay). + +- Each L6470 acts on the **last** SPI data it saw when the `SS_PIN` **goes high**. + +The L6474 uses the standard STEP DIR interface. Phase currents are changed in response to step pulses. The direction is set by the DIR pin. Instead of an ENA pin, stepper power is controlled with SPI commands. + +The other drivers operate in `STEP_CLOCK` mode. In this mode the Direction / Enable functions are done with SPI commands and the phase currents are changed in response to STEP pulses. + +### Hardware / Software Interaction + +Except for the L6474, powering up a stepper and setting the direction are done by the same command. You can't do one without the other. + +**All** directions are set **every time** a new block is popped off the queue by the stepper ISR. + +When setting direction, SPI transfers are minimized by using arrays and a specialized SPI method. *Arduino-L6470* library calls are not used. For N L64xx drivers, this results in N bytes transferred. If library calls were used then N<sup>2</sup> bytes would be sent. + +### Power-up (Reset) Sequence + +- Stepper objects are instantiated before the `setup()` entry point is reached. + +- In `setup()` (before stepper drivers are initialized) the `L6470_init()` method is called to do the following: + + - If present, pulse the hardware reset pin. + + - Populate the `L6470_chain` array, which maps positions in the SPI stream to commands/data for L64XX stepper drivers. + + - Initialize the L64XX Software SPI pin states. + + - Initialize L64XX drivers. They may be reset later by a call to `L6470_init_to_defaults()`. + +The steppers are **NOT** powered up (enabled) during this sequence. + +### `L6470_chain` array + +This array is used by all routines that transmit SPI data. For a chain with N devices, the array contains: + +Index|Value +-----|----- +0|Number of drivers in chain +1|Axis index of the first device in the chain (closest to MOSI) +...| +N|Axis index of the last device chain (closest to MISO) + +### Set Direction and Enable + +The `DIR_WRITE` macros for the L64xx drivers are written so that the standard X, Y, Z and extruder logic used by the `set_directions()` routine is not altered. These macros write the correct forward/reverse command to the corresponding location in the array `L6470_dir_commands`. On the L6474 the array the command used just enables the stepper because direction is set by the DIR pin. + +At the end of the `set_directions()` routine, the array `L6470_chain` is used to grab the corresponding direction/enable commands out of the array `L6470_dir_commands` and put them in the correct sequence in the array `L6470_buf`. Array `L6470_buf` is then passed to the **`void`** `L6470_Transfer` function which actually sends the data to the devices. + +### Utilities, etc. + +The **absolute position** registers should accurately reflect Marlin’s stepper position counts. They are set to zero during initialization. `G28` sets them to the Marlin counts for the corresponding axis after homing. NOTE: These registers are often the negative of the Marlin counts. This is because the Marlin counts reflect the logical direction while the registers reflect the stepper direction. The register contents are displayed via the `M114 D` command. + +The `L6470_monitor` feature reads the status of each device every half second. It will report if there are any error conditions present or if communications has been lost/restored. The `KVAL_HOLD` value is reduced every 2 – 2.5 seconds if the thermal warning or thermal shutdown conditions are present. + +**M122** displays the settings of most of the bits in the status register plus a couple of other items. + +**M906** can be used to set the `KVAL_HOLD` register (`TVAL` on L6474) one driver at a time. If a setting is not included with the command then the contents of the registers that affect the phase current/voltage are displayed. + +**M916, M917 & M918** + +These utilities are used to tune the system. They can get you in the ballpark for acceptable jerk, acceleration, top speed and `KVAL_HOLD` settings (`TVAL` on L6474). In general they seem to provide an overly optimistic `KVAL_HOLD` (`TVAL`) setting because of the lag between setting `KVAL_HOLD` (`TVAL`) and the driver reaching final temperature. Enabling the `L6470_monitor` feature during prints will provide the **final useful setting**. + +The amount of power needed to move the stepper without skipping steps increases as jerk, acceleration, top speed, and micro-steps increase. The power dissipated by the driver increases as the power to the stepper increases. The net result is a balancing act between jerk, acceleration, top speed, micro-steps, and power dissipated by the driver. + +**M916** - Increases `KVAL_HOLD` (`TVAL`) while moving one axis until a thermal warning is generated. This routine is also useful for determining the approximate `KVAL_HOLD` (`TVAL`) where the stepper stops losing steps. The sound will get noticeably quieter as it stops losing steps. + +**M917** - Find minimum current thresholds. This is accomplished by doing the following steps while moving an axis: + +1. Decrease OCD current until overcurrent error. + +2. Increase OCD until overcurrent error goes away. + +3. Decrease stall threshold until stall error (not available on the L6474). + +4. Increase stall until stall error goes away (not available on the L6474). + +**M918** - Increase speed until error or max feedrate achieved. diff --git a/Marlin/src/libs/W25Qxx.cpp b/Marlin/src/libs/W25Qxx.cpp new file mode 100644 index 0000000..be5b429 --- /dev/null +++ b/Marlin/src/libs/W25Qxx.cpp @@ -0,0 +1,395 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../inc/MarlinConfig.h" + +#if HAS_SPI_FLASH + +#include "W25Qxx.h" + +W25QXXFlash W25QXX; + +#ifndef SPI_FLASH_MISO_PIN + #define SPI_FLASH_MISO_PIN W25QXX_MISO_PIN +#endif +#ifndef SPI_FLASH_MOSI_PIN + #define SPI_FLASH_MOSI_PIN W25QXX_MOSI_PIN +#endif +#ifndef SPI_FLASH_SCK_PIN + #define SPI_FLASH_SCK_PIN W25QXX_SCK_PIN +#endif +#ifndef SPI_FLASH_CS_PIN + #define SPI_FLASH_CS_PIN W25QXX_CS_PIN +#endif +#ifndef NC + #define NC -1 +#endif + +MarlinSPI W25QXXFlash::mySPI(SPI_FLASH_MOSI_PIN, SPI_FLASH_MISO_PIN, SPI_FLASH_SCK_PIN, NC); + +#define W25QXX_CS_H OUT_WRITE(SPI_FLASH_CS_PIN, HIGH) +#define W25QXX_CS_L OUT_WRITE(SPI_FLASH_CS_PIN, LOW) + +bool flash_dma_mode = true; + +void W25QXXFlash::init(uint8_t spiRate) { + + OUT_WRITE(SPI_FLASH_CS_PIN, HIGH); + + /** + * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz + * STM32F1 has 3 SPI ports, SPI1 in APB2, SPI2/SPI3 in APB1 + * so the minimum prescale of SPI1 is DIV4, SPI2/SPI3 is DIV2 + */ + #if SPI_DEVICE == 1 + #define SPI_CLOCK_MAX SPI_CLOCK_DIV4 + #else + #define SPI_CLOCK_MAX SPI_CLOCK_DIV2 + #endif + uint8_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = SPI_CLOCK_MAX; break; + case SPI_HALF_SPEED: clock = SPI_CLOCK_DIV4; break; + case SPI_QUARTER_SPEED: clock = SPI_CLOCK_DIV8; break; + case SPI_EIGHTH_SPEED: clock = SPI_CLOCK_DIV16; break; + case SPI_SPEED_5: clock = SPI_CLOCK_DIV32; break; + case SPI_SPEED_6: clock = SPI_CLOCK_DIV64; break; + default: clock = SPI_CLOCK_DIV2;// Default from the SPI library + } + + mySPI.setClockDivider(clock); + mySPI.setBitOrder(MSBFIRST); + mySPI.setDataMode(SPI_MODE0); + mySPI.begin(); +} + +/** + * @brief Receive a single byte from the SPI port. + * + * @return Byte received + */ +uint8_t W25QXXFlash::spi_flash_Rec() { + const uint8_t returnByte = mySPI.transfer(0xFF); + return returnByte; +} + +uint8_t W25QXXFlash::spi_flash_read_write_byte(uint8_t data) { + const uint8_t returnByte = mySPI.transfer(data); + return returnByte; +} + +/** + * @brief Receive a number of bytes from the SPI port to a buffer + * + * @param buf Pointer to starting address of buffer to write to. + * @param nbyte Number of bytes to receive. + * @return Nothing + * + * @details Uses DMA + */ +void W25QXXFlash::spi_flash_Read(uint8_t* buf, uint16_t nbyte) { + mySPI.dmaTransfer(0, const_cast<uint8_t*>(buf), nbyte); +} + +/** + * @brief Send a single byte on SPI port + * + * @param b Byte to send + * + * @details + */ +void W25QXXFlash::spi_flash_Send(uint8_t b) { mySPI.transfer(b); } + +/** + * @brief Write token and then write from 512 byte buffer to SPI (for SD card) + * + * @param buf Pointer with buffer start address + * @return Nothing + * + * @details Use DMA + */ +void W25QXXFlash::spi_flash_SendBlock(uint8_t token, const uint8_t* buf) { + mySPI.transfer(token); + mySPI.dmaSend(const_cast<uint8_t*>(buf), 512); +} + +uint16_t W25QXXFlash::W25QXX_ReadID(void) { + uint16_t Temp = 0; + W25QXX_CS_L; + spi_flash_Send(0x90); + spi_flash_Send(0x00); + spi_flash_Send(0x00); + spi_flash_Send(0x00); + Temp |= spi_flash_Rec() << 8; + Temp |= spi_flash_Rec(); + W25QXX_CS_H; + return Temp; +} + +void W25QXXFlash::SPI_FLASH_WriteEnable(void) { + // Select the FLASH: Chip Select low + W25QXX_CS_L; + // Send "Write Enable" instruction + spi_flash_Send(W25X_WriteEnable); + // Deselect the FLASH: Chip Select high + W25QXX_CS_H; +} + +/******************************************************************************* +* Function Name : SPI_FLASH_WaitForWriteEnd +* Description : Polls the status of the Write In Progress (WIP) flag in the +* FLASH's status register and loop until write opertaion +* has completed. +* Input : None +* Output : None +* Return : None +*******************************************************************************/ +void W25QXXFlash::SPI_FLASH_WaitForWriteEnd(void) { + uint8_t FLASH_Status = 0; + + // Select the FLASH: Chip Select low + W25QXX_CS_L; + // Send "Read Status Register" instruction + spi_flash_Send(W25X_ReadStatusReg); + + // Loop as long as the memory is busy with a write cycle + do + /* Send a dummy byte to generate the clock needed by the FLASH + and put the value of the status register in FLASH_Status variable */ + FLASH_Status = spi_flash_Rec(); + while ((FLASH_Status & WIP_Flag) == 0x01); // Write in progress + + // Deselect the FLASH: Chip Select high + W25QXX_CS_H; +} + +void W25QXXFlash::SPI_FLASH_SectorErase(uint32_t SectorAddr) { + // Send write enable instruction + SPI_FLASH_WriteEnable(); + + // Sector Erase + // Select the FLASH: Chip Select low + W25QXX_CS_L; + // Send Sector Erase instruction + spi_flash_Send(W25X_SectorErase); + // Send SectorAddr high nibble address byte + spi_flash_Send((SectorAddr & 0xFF0000) >> 16); + // Send SectorAddr medium nibble address byte + spi_flash_Send((SectorAddr & 0xFF00) >> 8); + // Send SectorAddr low nibble address byte + spi_flash_Send(SectorAddr & 0xFF); + // Deselect the FLASH: Chip Select high + + W25QXX_CS_H; + // Wait the end of Flash writing + SPI_FLASH_WaitForWriteEnd(); +} + +void W25QXXFlash::SPI_FLASH_BlockErase(uint32_t BlockAddr) { + SPI_FLASH_WriteEnable(); + W25QXX_CS_L; + // Send Sector Erase instruction + spi_flash_Send(W25X_BlockErase); + // Send SectorAddr high nibble address byte + spi_flash_Send((BlockAddr & 0xFF0000) >> 16); + // Send SectorAddr medium nibble address byte + spi_flash_Send((BlockAddr & 0xFF00) >> 8); + // Send SectorAddr low nibble address byte + spi_flash_Send(BlockAddr & 0xFF); + + W25QXX_CS_H; + + SPI_FLASH_WaitForWriteEnd(); +} + +/******************************************************************************* +* Function Name : SPI_FLASH_BulkErase +* Description : Erases the entire FLASH. +* Input : None +* Output : None +* Return : None +*******************************************************************************/ +void W25QXXFlash::SPI_FLASH_BulkErase(void) { + // Send write enable instruction + SPI_FLASH_WriteEnable(); + + // Bulk Erase + // Select the FLASH: Chip Select low + W25QXX_CS_L; + + // Send Bulk Erase instruction + spi_flash_Send(W25X_ChipErase); + // Deselect the FLASH: Chip Select high + W25QXX_CS_H; + // Wait the end of Flash writing + SPI_FLASH_WaitForWriteEnd(); +} + +/******************************************************************************* +* Function Name : SPI_FLASH_PageWrite +* Description : Writes more than one byte to the FLASH with a single WRITE +* cycle(Page WRITE sequence). The number of byte can't exceed +* the FLASH page size. +* Input : - pBuffer : pointer to the buffer containing the data to be +* written to the FLASH. +* - WriteAddr : FLASH's internal address to write to. +* - NumByteToWrite : number of bytes to write to the FLASH, +* must be equal or less than "SPI_FLASH_PageSize" value. +* Output : None +* Return : None +*******************************************************************************/ +void W25QXXFlash::SPI_FLASH_PageWrite(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite) { + // Enable the write access to the FLASH + SPI_FLASH_WriteEnable(); + + // Select the FLASH: Chip Select low + W25QXX_CS_L; + // Send "Write to Memory " instruction + spi_flash_Send(W25X_PageProgram); + // Send WriteAddr high nibble address byte to write to + spi_flash_Send((WriteAddr & 0xFF0000) >> 16); + // Send WriteAddr medium nibble address byte to write to + spi_flash_Send((WriteAddr & 0xFF00) >> 8); + // Send WriteAddr low nibble address byte to write to + spi_flash_Send(WriteAddr & 0xFF); + + NOMORE(NumByteToWrite, SPI_FLASH_PerWritePageSize); + + // While there is data to be written on the FLASH + while (NumByteToWrite--) { + // Send the current byte + spi_flash_Send(*pBuffer); + // Point on the next byte to be written + pBuffer++; + } + + // Deselect the FLASH: Chip Select high + W25QXX_CS_H; + + // Wait the end of Flash writing + SPI_FLASH_WaitForWriteEnd(); +} + +/******************************************************************************* +* Function Name : SPI_FLASH_BufferWrite +* Description : Writes block of data to the FLASH. In this function, the +* number of WRITE cycles are reduced, using Page WRITE sequence. +* Input : - pBuffer : pointer to the buffer containing the data to be +* written to the FLASH. +* - WriteAddr : FLASH's internal address to write to. +* - NumByteToWrite : number of bytes to write to the FLASH. +* Output : None +* Return : None +*******************************************************************************/ +void W25QXXFlash::SPI_FLASH_BufferWrite(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite) { + uint8_t NumOfPage = 0, NumOfSingle = 0, Addr = 0, count = 0, temp = 0; + + Addr = WriteAddr % SPI_FLASH_PageSize; + count = SPI_FLASH_PageSize - Addr; + NumOfPage = NumByteToWrite / SPI_FLASH_PageSize; + NumOfSingle = NumByteToWrite % SPI_FLASH_PageSize; + + if (Addr == 0) { // WriteAddr is SPI_FLASH_PageSize aligned + if (NumOfPage == 0) { // NumByteToWrite < SPI_FLASH_PageSize + SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumByteToWrite); + } + else { // NumByteToWrite > SPI_FLASH_PageSize + while (NumOfPage--) { + SPI_FLASH_PageWrite(pBuffer, WriteAddr, SPI_FLASH_PageSize); + WriteAddr += SPI_FLASH_PageSize; + pBuffer += SPI_FLASH_PageSize; + } + SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumOfSingle); + } + } + else { // WriteAddr is not SPI_FLASH_PageSize aligned + if (NumOfPage == 0) { // NumByteToWrite < SPI_FLASH_PageSize + if (NumOfSingle > count) { // (NumByteToWrite + WriteAddr) > SPI_FLASH_PageSize + temp = NumOfSingle - count; + SPI_FLASH_PageWrite(pBuffer, WriteAddr, count); + WriteAddr += count; + pBuffer += count; + SPI_FLASH_PageWrite(pBuffer, WriteAddr, temp); + } + else + SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumByteToWrite); + } + else { // NumByteToWrite > SPI_FLASH_PageSize + NumByteToWrite -= count; + NumOfPage = NumByteToWrite / SPI_FLASH_PageSize; + NumOfSingle = NumByteToWrite % SPI_FLASH_PageSize; + + SPI_FLASH_PageWrite(pBuffer, WriteAddr, count); + WriteAddr += count; + pBuffer += count; + + while (NumOfPage--) { + SPI_FLASH_PageWrite(pBuffer, WriteAddr, SPI_FLASH_PageSize); + WriteAddr += SPI_FLASH_PageSize; + pBuffer += SPI_FLASH_PageSize; + } + + if (NumOfSingle != 0) + SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumOfSingle); + } + } +} + +/******************************************************************************* +* Function Name : SPI_FLASH_BufferRead +* Description : Reads a block of data from the FLASH. +* Input : - pBuffer : pointer to the buffer that receives the data read +* from the FLASH. +* - ReadAddr : FLASH's internal address to read from. +* - NumByteToRead : number of bytes to read from the FLASH. +* Output : None +* Return : None +*******************************************************************************/ +void W25QXXFlash::SPI_FLASH_BufferRead(uint8_t* pBuffer, uint32_t ReadAddr, uint16_t NumByteToRead) { + // Select the FLASH: Chip Select low + W25QXX_CS_L; + + // Send "Read from Memory " instruction + spi_flash_Send(W25X_ReadData); + + // Send ReadAddr high nibble address byte to read from + spi_flash_Send((ReadAddr & 0xFF0000) >> 16); + // Send ReadAddr medium nibble address byte to read from + spi_flash_Send((ReadAddr & 0xFF00) >> 8); + // Send ReadAddr low nibble address byte to read from + spi_flash_Send(ReadAddr & 0xFF); + + if (NumByteToRead <= 32 || !flash_dma_mode) { + while (NumByteToRead--) { // While there is data to be read + // Read a byte from the FLASH + *pBuffer = spi_flash_Rec(); + // Point to the next location where the byte read will be saved + pBuffer++; + } + } + else + spi_flash_Read(pBuffer, NumByteToRead); + + W25QXX_CS_H; +} + +#endif // HAS_SPI_FLASH diff --git a/Marlin/src/libs/W25Qxx.h b/Marlin/src/libs/W25Qxx.h new file mode 100644 index 0000000..eddae6b --- /dev/null +++ b/Marlin/src/libs/W25Qxx.h @@ -0,0 +1,74 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include <stdint.h> + +#include HAL_PATH(../HAL, MarlinSPI.h) + +#define W25X_WriteEnable 0x06 +#define W25X_WriteDisable 0x04 +#define W25X_ReadStatusReg 0x05 +#define W25X_WriteStatusReg 0x01 +#define W25X_ReadData 0x03 +#define W25X_FastReadData 0x0B +#define W25X_FastReadDual 0x3B +#define W25X_PageProgram 0x02 +#define W25X_BlockErase 0xD8 +#define W25X_SectorErase 0x20 +#define W25X_ChipErase 0xC7 +#define W25X_PowerDown 0xB9 +#define W25X_ReleasePowerDown 0xAB +#define W25X_DeviceID 0xAB +#define W25X_ManufactDeviceID 0x90 +#define W25X_JedecDeviceID 0x9F + +#define WIP_Flag 0x01 /* Write In Progress (WIP) flag */ + +#define Dummy_Byte 0xA5 + +#define SPI_FLASH_SectorSize 4096 +#define SPI_FLASH_PageSize 256 +#define SPI_FLASH_PerWritePageSize 256 + +class W25QXXFlash { +private: + static MarlinSPI mySPI; +public: + void init(uint8_t spiRate); + static uint8_t spi_flash_Rec(); + static uint8_t spi_flash_read_write_byte(uint8_t data); + static void spi_flash_Read(uint8_t* buf, uint16_t nbyte); + static void spi_flash_Send(uint8_t b); + static void spi_flash_SendBlock(uint8_t token, const uint8_t* buf); + static uint16_t W25QXX_ReadID(void); + static void SPI_FLASH_WriteEnable(void); + static void SPI_FLASH_WaitForWriteEnd(void); + static void SPI_FLASH_SectorErase(uint32_t SectorAddr); + static void SPI_FLASH_BlockErase(uint32_t BlockAddr); + static void SPI_FLASH_BulkErase(void); + static void SPI_FLASH_PageWrite(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite); + static void SPI_FLASH_BufferWrite(uint8_t* pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite); + static void SPI_FLASH_BufferRead(uint8_t* pBuffer, uint32_t ReadAddr, uint16_t NumByteToRead); +}; + +extern W25QXXFlash W25QXX; diff --git a/Marlin/src/libs/autoreport.h b/Marlin/src/libs/autoreport.h new file mode 100644 index 0000000..2c0a043 --- /dev/null +++ b/Marlin/src/libs/autoreport.h @@ -0,0 +1,49 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../inc/MarlinConfig.h" + +template <typename Helper> +struct AutoReporter { + millis_t next_report_ms; + uint8_t report_interval; + #if HAS_MULTI_SERIAL + serial_index_t report_port_mask; + AutoReporter() : report_port_mask(SERIAL_ALL) {} + #endif + + inline void set_interval(uint8_t seconds, const uint8_t limit=60) { + report_interval = _MIN(seconds, limit); + next_report_ms = millis() + SEC_TO_MS(seconds); + } + + inline void tick() { + if (!report_interval) return; + const millis_t ms = millis(); + if (ELAPSED(ms, next_report_ms)) { + next_report_ms = ms + SEC_TO_MS(report_interval); + TERN_(HAS_MULTI_SERIAL, PORT_REDIRECT(report_port_mask)); + Helper::report(); + } + } +}; diff --git a/Marlin/src/libs/bresenham.h b/Marlin/src/libs/bresenham.h new file mode 100644 index 0000000..ade231e --- /dev/null +++ b/Marlin/src/libs/bresenham.h @@ -0,0 +1,132 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../core/serial.h" + +/** + * bresenham_t.h - Bresenham algorithm template + * + * An array of values / counters that tick together + */ + +#define FORCE_INLINE __attribute__((always_inline)) inline +#define _O3 __attribute__((optimize("O3"))) + +template <uint8_t uid, uint8_t size> +struct BresenhamCfg { static constexpr uint8_t UID = uid, SIZE = size; }; + +template<typename T, typename Cfg> +class Bresenham { +private: + + static constexpr T signtest = -1; + static_assert(signtest < 0, "Bresenham type must be signed!"); + +public: + + static T divisor, value[Cfg::SIZE], dir[Cfg::SIZE], dividend[Cfg::SIZE], counter[Cfg::SIZE]; + + // Default: Instantiate all items with the identical parameters + Bresenham(const T &inDivisor=1, const int8_t &inDir=1, const T &inDividend=1, const T &inValue=0) { + for (uint8_t i = 0; i < Cfg::SIZE; i++) init(i, inDivisor, inDir, inDividend, inValue); + } + + // Instantiate all items with the same divisor + Bresenham(const T &inDivisor, const int8_t (&inDir)[Cfg::SIZE], const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) { + init(inDivisor, inDir, inDividend, inValue); + } + + // Instantiate all items with the same divisor and direction + Bresenham(const T &inDivisor, const int8_t &inDir, const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) { + init(inDivisor, inDir, inDividend, inValue); + } + + // Init all items with the same parameters + FORCE_INLINE static void init(const uint8_t index, const T &inDivisor=1, const int8_t &inDir=1, const T &inDividend=1, const T &inValue=0) { + divisor = inDivisor; + dir[index] = inDir; + dividend[index] = inDividend; + value[index] = inValue; + prime(index); + } + + // Init all items with the same divisor + FORCE_INLINE static void init(const T &inDivisor, const int8_t (&inDir)[Cfg::SIZE], const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) { + divisor = inDivisor; + for (uint8_t i = 0; i < Cfg::SIZE; i++) { + dir[i] = inDir[i]; + dividend[i] = inDividend[i]; + value[i] = inValue[i]; + } + prime(); + } + + // Init all items with the same divisor and direction + FORCE_INLINE static void init(const T &inDivisor, const int8_t &inDir, const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) { + divisor = inDivisor; + for (uint8_t i = 0; i < Cfg::SIZE; i++) { + dir[i] = inDir; + dividend[i] = inDividend[i]; + value[i] = inValue[i]; + } + prime(); + } + + // Reinit item with new dir, dividend, value keeping the same divisor + FORCE_INLINE static void reinit(const uint8_t index, const int8_t &inDir=1, const T &inDividend=1, const T &inValue=0) { + dir[index] = inDir; + dividend[index] = inDividend; + value[index] = inValue; + prime(); + } + + FORCE_INLINE static void prime(const uint8_t index) { counter[index] = -(divisor / 2); } + FORCE_INLINE static void prime() { for (uint8_t i = 0; i < Cfg::SIZE; i++) prime(i); } + + FORCE_INLINE static void back(const uint8_t index) { counter[index] -= divisor; } + + FORCE_INLINE static bool tick1(const uint8_t index) { + counter[index] += dividend[index]; + return counter[index] > 0; + } + + FORCE_INLINE static void tick(const uint8_t index) { + if (tick1(index)) { value[index] += dir[index]; back(index); } + } + + FORCE_INLINE static void tick1() _O3 { for (uint8_t i = 0; i < Cfg::SIZE; i++) (void)tick1(i); } + + FORCE_INLINE static void tick() _O3 { for (uint8_t i = 0; i < Cfg::SIZE; i++) (void)tick(i); } + + static void report(const uint8_t index) { + if (index < Cfg::SIZE) { + SERIAL_ECHOPAIR("bresenham ", int(index), " : (", dividend[index], "/", divisor, ") "); + if (counter[index] >= 0) SERIAL_CHAR(' '); + if (labs(counter[index]) < 100) { SERIAL_CHAR(' '); if (labs(counter[index]) < 10) SERIAL_CHAR(' '); } + SERIAL_ECHO(counter[index]); + SERIAL_ECHOLNPAIR(" ... ", value[index]); + } + } + + static void report() { for (uint8_t i = 0; i < Cfg::SIZE; i++) report(i); } +}; diff --git a/Marlin/src/libs/buzzer.cpp b/Marlin/src/libs/buzzer.cpp new file mode 100644 index 0000000..57ed5fb --- /dev/null +++ b/Marlin/src/libs/buzzer.cpp @@ -0,0 +1,84 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../inc/MarlinConfig.h" + +#if USE_BEEPER + +#include "buzzer.h" +#include "../module/temperature.h" +#include "../lcd/marlinui.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../lcd/extui/ui_api.h" +#endif + +Buzzer::state_t Buzzer::state; +CircularQueue<tone_t, TONE_QUEUE_LENGTH> Buzzer::buffer; +Buzzer buzzer; + +/** + * @brief Add a tone to the queue + * @details Adds a tone_t structure to the ring buffer, will block IO if the + * queue is full waiting for one slot to get available. + * + * @param duration Duration of the tone in milliseconds + * @param frequency Frequency of the tone in hertz + */ +void Buzzer::tone(const uint16_t duration, const uint16_t frequency/*=0*/) { + if (!ui.buzzer_enabled) return; + while (buffer.isFull()) { + tick(); + thermalManager.manage_heater(); + } + tone_t tone = { duration, frequency }; + buffer.enqueue(tone); +} + +void Buzzer::tick() { + if (!ui.buzzer_enabled) return; + const millis_t now = millis(); + + if (!state.endtime) { + if (buffer.isEmpty()) return; + + state.tone = buffer.dequeue(); + state.endtime = now + state.tone.duration; + + if (state.tone.frequency > 0) { + #if ENABLED(EXTENSIBLE_UI) && DISABLED(EXTUI_LOCAL_BEEPER) + CRITICAL_SECTION_START(); + ExtUI::onPlayTone(state.tone.frequency, state.tone.duration); + CRITICAL_SECTION_END(); + #elif ENABLED(SPEAKER) + CRITICAL_SECTION_START(); + ::tone(BEEPER_PIN, state.tone.frequency, state.tone.duration); + CRITICAL_SECTION_END(); + #else + on(); + #endif + } + } + else if (ELAPSED(now, state.endtime)) reset(); +} + +#endif // USE_BEEPER diff --git a/Marlin/src/libs/buzzer.h b/Marlin/src/libs/buzzer.h new file mode 100644 index 0000000..b86fe99 --- /dev/null +++ b/Marlin/src/libs/buzzer.h @@ -0,0 +1,129 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../inc/MarlinConfig.h" + +#if USE_BEEPER + + #include "circularqueue.h" + + #define TONE_QUEUE_LENGTH 4 + + /** + * @brief Tone structure + * @details Simple abstraction of a tone based on a duration and a frequency. + */ + struct tone_t { + uint16_t duration; + uint16_t frequency; + }; + + /** + * @brief Buzzer class + */ + class Buzzer { + public: + + typedef struct { + tone_t tone; + uint32_t endtime; + } state_t; + + private: + static state_t state; + + protected: + static CircularQueue<tone_t, TONE_QUEUE_LENGTH> buffer; + + /** + * @brief Inverts the sate of a digital PIN + * @details This will invert the current state of an digital IO pin. + */ + FORCE_INLINE static void invert() { TOGGLE(BEEPER_PIN); } + + /** + * @brief Turn off a digital PIN + * @details Alias of digitalWrite(PIN, LOW) using FastIO + */ + FORCE_INLINE static void off() { WRITE(BEEPER_PIN, LOW); } + + /** + * @brief Turn on a digital PIN + * @details Alias of digitalWrite(PIN, HIGH) using FastIO + */ + FORCE_INLINE static void on() { WRITE(BEEPER_PIN, HIGH); } + + /** + * @brief Resets the state of the class + * @details Brings the class state to a known one. + */ + static inline void reset() { + off(); + state.endtime = 0; + } + + public: + /** + * @brief Init Buzzer + */ + static inline void init() { + SET_OUTPUT(BEEPER_PIN); + reset(); + } + + /** + * @brief Add a tone to the queue + * @details Adds a tone_t structure to the ring buffer, will block IO if the + * queue is full waiting for one slot to get available. + * + * @param duration Duration of the tone in milliseconds + * @param frequency Frequency of the tone in hertz + */ + static void tone(const uint16_t duration, const uint16_t frequency=0); + + /** + * @brief Tick function + * @details This function should be called at loop, it will take care of + * playing the tones in the queue. + */ + static void tick(); + }; + + // Provide a buzzer instance + extern Buzzer buzzer; + + // Buzz directly via the BEEPER pin tone queue + #define BUZZ(d,f) buzzer.tone(d, f) + +#elif HAS_BUZZER + + // Buzz indirectly via the MarlinUI instance + #include "../lcd/marlinui.h" + #define BUZZ(d,f) ui.buzz(d,f) + +#else + + // No buzz capability + #define BUZZ(d,f) NOOP + +#endif diff --git a/Marlin/src/libs/circularqueue.h b/Marlin/src/libs/circularqueue.h new file mode 100644 index 0000000..4d4a464 --- /dev/null +++ b/Marlin/src/libs/circularqueue.h @@ -0,0 +1,131 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include <stdint.h> + +/** + * @brief Circular Queue class + * @details Implementation of the classic ring buffer data structure + */ +template<typename T, uint8_t N> +class CircularQueue { + private: + + /** + * @brief Buffer structure + * @details This structure consolidates all the overhead required to handle + * a circular queue such as the pointers and the buffer vector. + */ + struct buffer_t { + uint8_t head; + uint8_t tail; + uint8_t count; + uint8_t size; + T queue[N]; + } buffer; + + public: + /** + * @brief Class constructor + * @details This class requires two template parameters, T defines the type + * of item this queue will handle and N defines the maximum number of + * items that can be stored on the queue. + */ + CircularQueue<T, N>() { + buffer.size = N; + buffer.count = buffer.head = buffer.tail = 0; + } + + /** + * @brief Removes and returns a item from the queue + * @details Removes the oldest item on the queue, pointed to by the + * buffer_t head field. The item is returned to the caller. + * @return type T item + */ + T dequeue() { + if (isEmpty()) return T(); + + uint8_t index = buffer.head; + + --buffer.count; + if (++buffer.head == buffer.size) + buffer.head = 0; + + return buffer.queue[index]; + } + + /** + * @brief Adds an item to the queue + * @details Adds an item to the queue on the location pointed by the buffer_t + * tail variable. Returns false if no queue space is available. + * @param item Item to be added to the queue + * @return true if the operation was successful + */ + bool enqueue(T const &item) { + if (isFull()) return false; + + buffer.queue[buffer.tail] = item; + + ++buffer.count; + if (++buffer.tail == buffer.size) + buffer.tail = 0; + + return true; + } + + /** + * @brief Checks if the queue has no items + * @details Returns true if there are no items on the queue, false otherwise. + * @return true if queue is empty + */ + bool isEmpty() { return buffer.count == 0; } + + /** + * @brief Checks if the queue is full + * @details Returns true if the queue is full, false otherwise. + * @return true if queue is full + */ + bool isFull() { return buffer.count == buffer.size; } + + /** + * @brief Gets the queue size + * @details Returns the maximum number of items a queue can have. + * @return the queue size + */ + uint8_t size() { return buffer.size; } + + /** + * @brief Gets the next item from the queue without removing it + * @details Returns the next item in the queue without removing it + * or updating the pointers. + * @return first item in the queue + */ + T peek() { return buffer.queue[buffer.head]; } + + /** + * @brief Gets the number of items on the queue + * @details Returns the current number of items stored on the queue. + * @return number of items in the queue + */ + uint8_t count() { return buffer.count; } +}; diff --git a/Marlin/src/libs/crc16.cpp b/Marlin/src/libs/crc16.cpp new file mode 100644 index 0000000..c219561 --- /dev/null +++ b/Marlin/src/libs/crc16.cpp @@ -0,0 +1,32 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "crc16.h" + +void crc16(uint16_t *crc, const void * const data, uint16_t cnt) { + uint8_t *ptr = (uint8_t *)data; + while (cnt--) { + *crc = (uint16_t)(*crc ^ (uint16_t)(((uint16_t)*ptr++) << 8)); + for (uint8_t i = 0; i < 8; i++) + *crc = (uint16_t)((*crc & 0x8000) ? ((uint16_t)(*crc << 1) ^ 0x1021) : (*crc << 1)); + } +} diff --git a/Marlin/src/libs/crc16.h b/Marlin/src/libs/crc16.h new file mode 100644 index 0000000..1760ecd --- /dev/null +++ b/Marlin/src/libs/crc16.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include <stdint.h> + +void crc16(uint16_t *crc, const void * const data, uint16_t cnt); diff --git a/Marlin/src/libs/duration_t.h b/Marlin/src/libs/duration_t.h new file mode 100644 index 0000000..bd654b7 --- /dev/null +++ b/Marlin/src/libs/duration_t.h @@ -0,0 +1,175 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../HAL/shared/Marduino.h" + +struct duration_t { + /** + * @brief Duration is stored in seconds + */ + uint32_t value; + + /** + * @brief Constructor + */ + duration_t() + : duration_t(0) {}; + + /** + * @brief Constructor + * + * @param seconds The number of seconds + */ + duration_t(uint32_t const &seconds) { + this->value = seconds; + } + + /** + * @brief Equality comparison + * @details Overloads the equality comparison operator + * + * @param value The number of seconds to compare to + * @return True if both durations are equal + */ + bool operator==(const uint32_t &value) const { + return (this->value == value); + } + + /** + * @brief Inequality comparison + * @details Overloads the inequality comparison operator + * + * @param value The number of seconds to compare to + * @return False if both durations are equal + */ + bool operator!=(const uint32_t &value) const { + return ! this->operator==(value); + } + + /** + * @brief Formats the duration as years + * @return The number of years + */ + inline uint8_t year() const { + return this->day() / 365; + } + + /** + * @brief Formats the duration as days + * @return The number of days + */ + inline uint16_t day() const { + return this->hour() / 24; + } + + /** + * @brief Formats the duration as hours + * @return The number of hours + */ + inline uint32_t hour() const { + return this->minute() / 60; + } + + /** + * @brief Formats the duration as minutes + * @return The number of minutes + */ + inline uint32_t minute() const { + return this->second() / 60; + } + + /** + * @brief Formats the duration as seconds + * @return The number of seconds + */ + inline uint32_t second() const { + return this->value; + } + + #if GCC_VERSION <= 50000 + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wformat-overflow" + #endif + + /** + * @brief Formats the duration as a string + * @details String will be formated using a "full" representation of duration + * + * @param buffer The array pointed to must be able to accommodate 21 bytes + * + * Output examples: + * 123456789012345678901 (strlen) + * 135y 364d 23h 59m 59s + * 364d 23h 59m 59s + * 23h 59m 59s + * 59m 59s + * 59s + */ + char* toString(char * const buffer) const { + int y = this->year(), + d = this->day() % 365, + h = this->hour() % 24, + m = this->minute() % 60, + s = this->second() % 60; + + if (y) sprintf_P(buffer, PSTR("%iy %id %ih %im %is"), y, d, h, m, s); + else if (d) sprintf_P(buffer, PSTR("%id %ih %im %is"), d, h, m, s); + else if (h) sprintf_P(buffer, PSTR("%ih %im %is"), h, m, s); + else if (m) sprintf_P(buffer, PSTR("%im %is"), m, s); + else sprintf_P(buffer, PSTR("%is"), s); + return buffer; + } + + /** + * @brief Formats the duration as a string + * @details String will be formated using a "digital" representation of duration + * + * @param buffer The array pointed to must be able to accommodate 10 bytes + * + * Output examples: + * 123456789 (strlen) + * 99:59 + * 11d 12:33 + */ + uint8_t toDigital(char *buffer, bool with_days=false) const { + uint16_t h = uint16_t(this->hour()), + m = uint16_t(this->minute() % 60UL); + if (with_days) { + uint16_t d = this->day(); + sprintf_P(buffer, PSTR("%hud %02hu:%02hu"), d, h % 24, m); + return d >= 10 ? 9 : 8; + } + else if (h < 100) { + sprintf_P(buffer, PSTR("%02hu:%02hu"), h, m); + return 5; + } + else { + sprintf_P(buffer, PSTR("%hu:%02hu"), h, m); + return 6; + } + } + + #if GCC_VERSION <= 50000 + #pragma GCC diagnostic pop + #endif +}; diff --git a/Marlin/src/libs/heatshrink/LICENSE b/Marlin/src/libs/heatshrink/LICENSE new file mode 100644 index 0000000..a40fc72 --- /dev/null +++ b/Marlin/src/libs/heatshrink/LICENSE @@ -0,0 +1,14 @@ +Copyright (c) 2013-2015, Scott Vokes <vokes.s@gmail.com> +All rights reserved. + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. diff --git a/Marlin/src/libs/heatshrink/heatshrink_common.h b/Marlin/src/libs/heatshrink/heatshrink_common.h new file mode 100644 index 0000000..68653e4 --- /dev/null +++ b/Marlin/src/libs/heatshrink/heatshrink_common.h @@ -0,0 +1,20 @@ +/** + * libs/heatshrink/heatshrink_common.h + */ +#pragma once + +#define HEATSHRINK_AUTHOR "Scott Vokes <vokes.s@gmail.com>" +#define HEATSHRINK_URL "github.com/atomicobject/heatshrink" + +/* Version 0.4.1 */ +#define HEATSHRINK_VERSION_MAJOR 0 +#define HEATSHRINK_VERSION_MINOR 4 +#define HEATSHRINK_VERSION_PATCH 1 + +#define HEATSHRINK_MIN_WINDOW_BITS 4 +#define HEATSHRINK_MAX_WINDOW_BITS 15 + +#define HEATSHRINK_MIN_LOOKAHEAD_BITS 3 + +#define HEATSHRINK_LITERAL_MARKER 0x01 +#define HEATSHRINK_BACKREF_MARKER 0x00 diff --git a/Marlin/src/libs/heatshrink/heatshrink_config.h b/Marlin/src/libs/heatshrink/heatshrink_config.h new file mode 100644 index 0000000..90520f1 --- /dev/null +++ b/Marlin/src/libs/heatshrink/heatshrink_config.h @@ -0,0 +1,26 @@ +/** + * libs/heatshrink/heatshrink_config.h + */ +#pragma once + +// Should functionality assuming dynamic allocation be used? +#ifndef HEATSHRINK_DYNAMIC_ALLOC + //#define HEATSHRINK_DYNAMIC_ALLOC 1 +#endif + +#if HEATSHRINK_DYNAMIC_ALLOC + // Optional replacement of malloc/free + #define HEATSHRINK_MALLOC(SZ) malloc(SZ) + #define HEATSHRINK_FREE(P, SZ) free(P) +#else + // Required parameters for static configuration + #define HEATSHRINK_STATIC_INPUT_BUFFER_SIZE 32 + #define HEATSHRINK_STATIC_WINDOW_BITS 8 + #define HEATSHRINK_STATIC_LOOKAHEAD_BITS 4 +#endif + +// Turn on logging for debugging +#define HEATSHRINK_DEBUGGING_LOGS 0 + +// Use indexing for faster compression. (This requires additional space.) +#define HEATSHRINK_USE_INDEX 1 diff --git a/Marlin/src/libs/heatshrink/heatshrink_decoder.cpp b/Marlin/src/libs/heatshrink/heatshrink_decoder.cpp new file mode 100644 index 0000000..073a7ed --- /dev/null +++ b/Marlin/src/libs/heatshrink/heatshrink_decoder.cpp @@ -0,0 +1,384 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(BINARY_FILE_TRANSFER) + +/** + * libs/heatshrink/heatshrink_decoder.cpp + */ +#include "heatshrink_decoder.h" + +#include <stdlib.h> +#include <string.h> + +#pragma GCC optimize ("O3") + +/* States for the polling state machine. */ +typedef enum { + HSDS_TAG_BIT, /* tag bit */ + HSDS_YIELD_LITERAL, /* ready to yield literal byte */ + HSDS_BACKREF_INDEX_MSB, /* most significant byte of index */ + HSDS_BACKREF_INDEX_LSB, /* least significant byte of index */ + HSDS_BACKREF_COUNT_MSB, /* most significant byte of count */ + HSDS_BACKREF_COUNT_LSB, /* least significant byte of count */ + HSDS_YIELD_BACKREF /* ready to yield back-reference */ +} HSD_state; + +#if HEATSHRINK_DEBUGGING_LOGS + #include <stdio.h> + #include <ctype.h> + #include <assert.h> + #define LOG(...) fprintf(stderr, __VA_ARGS__) + #define ASSERT(X) assert(X) + static const char *state_names[] = { + "tag_bit", + "yield_literal", + "backref_index_msb", + "backref_index_lsb", + "backref_count_msb", + "backref_count_lsb", + "yield_backref" + }; +#else + #define LOG(...) /* no-op */ + #define ASSERT(X) /* no-op */ +#endif + +typedef struct { + uint8_t *buf; /* output buffer */ + size_t buf_size; /* buffer size */ + size_t *output_size; /* bytes pushed to buffer, so far */ +} output_info; + +#define NO_BITS ((uint16_t)-1) + +/* Forward references. */ +static uint16_t get_bits(heatshrink_decoder *hsd, uint8_t count); +static void push_byte(heatshrink_decoder *hsd, output_info *oi, uint8_t byte); + +#if HEATSHRINK_DYNAMIC_ALLOC +heatshrink_decoder *heatshrink_decoder_alloc(uint16_t input_buffer_size, uint8_t window_sz2, uint8_t lookahead_sz2) { + if ((window_sz2 < HEATSHRINK_MIN_WINDOW_BITS) || + (window_sz2 > HEATSHRINK_MAX_WINDOW_BITS) || + (input_buffer_size == 0) || + (lookahead_sz2 < HEATSHRINK_MIN_LOOKAHEAD_BITS) || + (lookahead_sz2 >= window_sz2)) { + return nullptr; + } + size_t buffers_sz = (1 << window_sz2) + input_buffer_size; + size_t sz = sizeof(heatshrink_decoder) + buffers_sz; + heatshrink_decoder *hsd = HEATSHRINK_MALLOC(sz); + if (!hsd) return nullptr; + hsd->input_buffer_size = input_buffer_size; + hsd->window_sz2 = window_sz2; + hsd->lookahead_sz2 = lookahead_sz2; + heatshrink_decoder_reset(hsd); + LOG("-- allocated decoder with buffer size of %zu (%zu + %u + %u)\n", + sz, sizeof(heatshrink_decoder), (1 << window_sz2), input_buffer_size); + return hsd; +} + +void heatshrink_decoder_free(heatshrink_decoder *hsd) { + size_t buffers_sz = (1 << hsd->window_sz2) + hsd->input_buffer_size; + size_t sz = sizeof(heatshrink_decoder) + buffers_sz; + HEATSHRINK_FREE(hsd, sz); + (void)sz; /* may not be used by free */ +} +#endif + +void heatshrink_decoder_reset(heatshrink_decoder *hsd) { + size_t buf_sz = 1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd); + size_t input_sz = HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd); + memset(hsd->buffers, 0, buf_sz + input_sz); + hsd->state = HSDS_TAG_BIT; + hsd->input_size = 0; + hsd->input_index = 0; + hsd->bit_index = 0x00; + hsd->current_byte = 0x00; + hsd->output_count = 0; + hsd->output_index = 0; + hsd->head_index = 0; +} + +/* Copy SIZE bytes into the decoder's input buffer, if it will fit. */ +HSD_sink_res heatshrink_decoder_sink(heatshrink_decoder *hsd, + uint8_t *in_buf, size_t size, size_t *input_size) { + if (!hsd || !in_buf || !input_size) + return HSDR_SINK_ERROR_NULL; + + size_t rem = HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd) - hsd->input_size; + if (rem == 0) { + *input_size = 0; + return HSDR_SINK_FULL; + } + + size = rem < size ? rem : size; + LOG("-- sinking %zd bytes\n", size); + /* copy into input buffer (at head of buffers) */ + memcpy(&hsd->buffers[hsd->input_size], in_buf, size); + hsd->input_size += size; + *input_size = size; + return HSDR_SINK_OK; +} + + +/***************** + * Decompression * + *****************/ + +#define BACKREF_COUNT_BITS(HSD) (HEATSHRINK_DECODER_LOOKAHEAD_BITS(HSD)) +#define BACKREF_INDEX_BITS(HSD) (HEATSHRINK_DECODER_WINDOW_BITS(HSD)) + +// States +static HSD_state st_tag_bit(heatshrink_decoder *hsd); +static HSD_state st_yield_literal(heatshrink_decoder *hsd, output_info *oi); +static HSD_state st_backref_index_msb(heatshrink_decoder *hsd); +static HSD_state st_backref_index_lsb(heatshrink_decoder *hsd); +static HSD_state st_backref_count_msb(heatshrink_decoder *hsd); +static HSD_state st_backref_count_lsb(heatshrink_decoder *hsd); +static HSD_state st_yield_backref(heatshrink_decoder *hsd, output_info *oi); + +HSD_poll_res heatshrink_decoder_poll(heatshrink_decoder *hsd, uint8_t *out_buf, size_t out_buf_size, size_t *output_size) { + if (!hsd || !out_buf || !output_size) + return HSDR_POLL_ERROR_NULL; + + *output_size = 0; + + output_info oi; + oi.buf = out_buf; + oi.buf_size = out_buf_size; + oi.output_size = output_size; + + while (1) { + LOG("-- poll, state is %d (%s), input_size %d\n", hsd->state, state_names[hsd->state], hsd->input_size); + uint8_t in_state = hsd->state; + switch (in_state) { + case HSDS_TAG_BIT: + hsd->state = st_tag_bit(hsd); + break; + case HSDS_YIELD_LITERAL: + hsd->state = st_yield_literal(hsd, &oi); + break; + case HSDS_BACKREF_INDEX_MSB: + hsd->state = st_backref_index_msb(hsd); + break; + case HSDS_BACKREF_INDEX_LSB: + hsd->state = st_backref_index_lsb(hsd); + break; + case HSDS_BACKREF_COUNT_MSB: + hsd->state = st_backref_count_msb(hsd); + break; + case HSDS_BACKREF_COUNT_LSB: + hsd->state = st_backref_count_lsb(hsd); + break; + case HSDS_YIELD_BACKREF: + hsd->state = st_yield_backref(hsd, &oi); + break; + default: + return HSDR_POLL_ERROR_UNKNOWN; + } + + // If the current state cannot advance, check if input or output + // buffer are exhausted. + if (hsd->state == in_state) + return (*output_size == out_buf_size) ? HSDR_POLL_MORE : HSDR_POLL_EMPTY; + } +} + +static HSD_state st_tag_bit(heatshrink_decoder *hsd) { + uint32_t bits = get_bits(hsd, 1); // get tag bit + if (bits == NO_BITS) + return HSDS_TAG_BIT; + else if (bits) + return HSDS_YIELD_LITERAL; + else if (HEATSHRINK_DECODER_WINDOW_BITS(hsd) > 8) + return HSDS_BACKREF_INDEX_MSB; + else { + hsd->output_index = 0; + return HSDS_BACKREF_INDEX_LSB; + } +} + +static HSD_state st_yield_literal(heatshrink_decoder *hsd, output_info *oi) { + /* Emit a repeated section from the window buffer, and add it (again) + * to the window buffer. (Note that the repetition can include + * itself.)*/ + if (*oi->output_size < oi->buf_size) { + uint16_t byte = get_bits(hsd, 8); + if (byte == NO_BITS) { return HSDS_YIELD_LITERAL; } /* out of input */ + uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)]; + uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1; + uint8_t c = byte & 0xFF; + LOG("-- emitting literal byte 0x%02x ('%c')\n", c, isprint(c) ? c : '.'); + buf[hsd->head_index++ & mask] = c; + push_byte(hsd, oi, c); + return HSDS_TAG_BIT; + } + return HSDS_YIELD_LITERAL; +} + +static HSD_state st_backref_index_msb(heatshrink_decoder *hsd) { + uint8_t bit_ct = BACKREF_INDEX_BITS(hsd); + ASSERT(bit_ct > 8); + uint16_t bits = get_bits(hsd, bit_ct - 8); + LOG("-- backref index (msb), got 0x%04x (+1)\n", bits); + if (bits == NO_BITS) { return HSDS_BACKREF_INDEX_MSB; } + hsd->output_index = bits << 8; + return HSDS_BACKREF_INDEX_LSB; +} + +static HSD_state st_backref_index_lsb(heatshrink_decoder *hsd) { + uint8_t bit_ct = BACKREF_INDEX_BITS(hsd); + uint16_t bits = get_bits(hsd, bit_ct < 8 ? bit_ct : 8); + LOG("-- backref index (lsb), got 0x%04x (+1)\n", bits); + if (bits == NO_BITS) { return HSDS_BACKREF_INDEX_LSB; } + hsd->output_index |= bits; + hsd->output_index++; + uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd); + hsd->output_count = 0; + return (br_bit_ct > 8) ? HSDS_BACKREF_COUNT_MSB : HSDS_BACKREF_COUNT_LSB; +} + +static HSD_state st_backref_count_msb(heatshrink_decoder *hsd) { + uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd); + ASSERT(br_bit_ct > 8); + uint16_t bits = get_bits(hsd, br_bit_ct - 8); + LOG("-- backref count (msb), got 0x%04x (+1)\n", bits); + if (bits == NO_BITS) { return HSDS_BACKREF_COUNT_MSB; } + hsd->output_count = bits << 8; + return HSDS_BACKREF_COUNT_LSB; +} + +static HSD_state st_backref_count_lsb(heatshrink_decoder *hsd) { + uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd); + uint16_t bits = get_bits(hsd, br_bit_ct < 8 ? br_bit_ct : 8); + LOG("-- backref count (lsb), got 0x%04x (+1)\n", bits); + if (bits == NO_BITS) { return HSDS_BACKREF_COUNT_LSB; } + hsd->output_count |= bits; + hsd->output_count++; + return HSDS_YIELD_BACKREF; +} + +static HSD_state st_yield_backref(heatshrink_decoder *hsd, output_info *oi) { + size_t count = oi->buf_size - *oi->output_size; + if (count > 0) { + size_t i = 0; + if (hsd->output_count < count) count = hsd->output_count; + uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)]; + uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1; + uint16_t neg_offset = hsd->output_index; + LOG("-- emitting %zu bytes from -%u bytes back\n", count, neg_offset); + ASSERT(neg_offset <= mask + 1); + ASSERT(count <= (size_t)(1 << BACKREF_COUNT_BITS(hsd))); + + for (i = 0; i < count; i++) { + uint8_t c = buf[(hsd->head_index - neg_offset) & mask]; + push_byte(hsd, oi, c); + buf[hsd->head_index & mask] = c; + hsd->head_index++; + LOG(" -- ++ 0x%02x\n", c); + } + hsd->output_count -= count; + if (hsd->output_count == 0) { return HSDS_TAG_BIT; } + } + return HSDS_YIELD_BACKREF; +} + +/* Get the next COUNT bits from the input buffer, saving incremental progress. + * Returns NO_BITS on end of input, or if more than 15 bits are requested. */ +static uint16_t get_bits(heatshrink_decoder *hsd, uint8_t count) { + uint16_t accumulator = 0; + int i = 0; + if (count > 15) return NO_BITS; + LOG("-- popping %u bit(s)\n", count); + + /* If we aren't able to get COUNT bits, suspend immediately, because we + * don't track how many bits of COUNT we've accumulated before suspend. */ + if (hsd->input_size == 0 && hsd->bit_index < (1 << (count - 1))) return NO_BITS; + + for (i = 0; i < count; i++) { + if (hsd->bit_index == 0x00) { + if (hsd->input_size == 0) { + LOG(" -- out of bits, suspending w/ accumulator of %u (0x%02x)\n", accumulator, accumulator); + return NO_BITS; + } + hsd->current_byte = hsd->buffers[hsd->input_index++]; + LOG(" -- pulled byte 0x%02x\n", hsd->current_byte); + if (hsd->input_index == hsd->input_size) { + hsd->input_index = 0; /* input is exhausted */ + hsd->input_size = 0; + } + hsd->bit_index = 0x80; + } + accumulator <<= 1; + if (hsd->current_byte & hsd->bit_index) { + accumulator |= 0x01; + if (0) { + LOG(" -- got 1, accumulator 0x%04x, bit_index 0x%02x\n", + accumulator, hsd->bit_index); + } + } + else if (0) { + LOG(" -- got 0, accumulator 0x%04x, bit_index 0x%02x\n", + accumulator, hsd->bit_index); + } + hsd->bit_index >>= 1; + } + + if (count > 1) LOG(" -- accumulated %08x\n", accumulator); + return accumulator; +} + +HSD_finish_res heatshrink_decoder_finish(heatshrink_decoder *hsd) { + if (!hsd) return HSDR_FINISH_ERROR_NULL; + switch (hsd->state) { + case HSDS_TAG_BIT: + return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE; + + /* If we want to finish with no input, but are in these states, it's + * because the 0-bit padding to the last byte looks like a backref + * marker bit followed by all 0s for index and count bits. */ + case HSDS_BACKREF_INDEX_LSB: + case HSDS_BACKREF_INDEX_MSB: + case HSDS_BACKREF_COUNT_LSB: + case HSDS_BACKREF_COUNT_MSB: + return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE; + + /* If the output stream is padded with 0xFFs (possibly due to being in + * flash memory), also explicitly check the input size rather than + * uselessly returning MORE but yielding 0 bytes when polling. */ + case HSDS_YIELD_LITERAL: + return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE; + + default: return HSDR_FINISH_MORE; + } +} + +static void push_byte(heatshrink_decoder *hsd, output_info *oi, uint8_t byte) { + LOG(" -- pushing byte: 0x%02x ('%c')\n", byte, isprint(byte) ? byte : '.'); + oi->buf[(*oi->output_size)++] = byte; + (void)hsd; +} + +#endif // BINARY_FILE_TRANSFER diff --git a/Marlin/src/libs/heatshrink/heatshrink_decoder.h b/Marlin/src/libs/heatshrink/heatshrink_decoder.h new file mode 100644 index 0000000..eb113aa --- /dev/null +++ b/Marlin/src/libs/heatshrink/heatshrink_decoder.h @@ -0,0 +1,119 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +/** + * libs/heatshrink/heatshrink_decoder.h + */ +#pragma once + +#include "heatshrink_common.h" +#include "heatshrink_config.h" + +#include <stdint.h> +#include <stddef.h> + +typedef enum { + HSDR_SINK_OK, /* data sunk, ready to poll */ + HSDR_SINK_FULL, /* out of space in internal buffer */ + HSDR_SINK_ERROR_NULL=-1, /* NULL argument */ +} HSD_sink_res; + +typedef enum { + HSDR_POLL_EMPTY, /* input exhausted */ + HSDR_POLL_MORE, /* more data remaining, call again w/ fresh output buffer */ + HSDR_POLL_ERROR_NULL=-1, /* NULL arguments */ + HSDR_POLL_ERROR_UNKNOWN=-2, +} HSD_poll_res; + +typedef enum { + HSDR_FINISH_DONE, /* output is done */ + HSDR_FINISH_MORE, /* more output remains */ + HSDR_FINISH_ERROR_NULL=-1, /* NULL arguments */ +} HSD_finish_res; + +#if HEATSHRINK_DYNAMIC_ALLOC +#define HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(BUF) \ + ((BUF)->input_buffer_size) +#define HEATSHRINK_DECODER_WINDOW_BITS(BUF) \ + ((BUF)->window_sz2) +#define HEATSHRINK_DECODER_LOOKAHEAD_BITS(BUF) \ + ((BUF)->lookahead_sz2) +#else +#define HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_) \ + HEATSHRINK_STATIC_INPUT_BUFFER_SIZE +#define HEATSHRINK_DECODER_WINDOW_BITS(_) \ + (HEATSHRINK_STATIC_WINDOW_BITS) +#define HEATSHRINK_DECODER_LOOKAHEAD_BITS(BUF) \ + (HEATSHRINK_STATIC_LOOKAHEAD_BITS) +#endif + +typedef struct { + uint16_t input_size; /* bytes in input buffer */ + uint16_t input_index; /* offset to next unprocessed input byte */ + uint16_t output_count; /* how many bytes to output */ + uint16_t output_index; /* index for bytes to output */ + uint16_t head_index; /* head of window buffer */ + uint8_t state; /* current state machine node */ + uint8_t current_byte; /* current byte of input */ + uint8_t bit_index; /* current bit index */ + +#if HEATSHRINK_DYNAMIC_ALLOC + /* Fields that are only used if dynamically allocated. */ + uint8_t window_sz2; /* window buffer bits */ + uint8_t lookahead_sz2; /* lookahead bits */ + uint16_t input_buffer_size; /* input buffer size */ + + /* Input buffer, then expansion window buffer */ + uint8_t buffers[]; +#else + /* Input buffer, then expansion window buffer */ + uint8_t buffers[(1 << HEATSHRINK_DECODER_WINDOW_BITS(_)) + HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_)]; +#endif +} heatshrink_decoder; + +#if HEATSHRINK_DYNAMIC_ALLOC +/* Allocate a decoder with an input buffer of INPUT_BUFFER_SIZE bytes, + * an expansion buffer size of 2^WINDOW_SZ2, and a lookahead + * size of 2^lookahead_sz2. (The window buffer and lookahead sizes + * must match the settings used when the data was compressed.) + * Returns NULL on error. */ +heatshrink_decoder *heatshrink_decoder_alloc(uint16_t input_buffer_size, uint8_t expansion_buffer_sz2, uint8_t lookahead_sz2); + +/* Free a decoder. */ +void heatshrink_decoder_free(heatshrink_decoder *hsd); +#endif + +/* Reset a decoder. */ +void heatshrink_decoder_reset(heatshrink_decoder *hsd); + +/* Sink at most SIZE bytes from IN_BUF into the decoder. *INPUT_SIZE is set to + * indicate how many bytes were actually sunk (in case a buffer was filled). */ +HSD_sink_res heatshrink_decoder_sink(heatshrink_decoder *hsd, uint8_t *in_buf, size_t size, size_t *input_size); + +/* Poll for output from the decoder, copying at most OUT_BUF_SIZE bytes into + * OUT_BUF (setting *OUTPUT_SIZE to the actual amount copied). */ +HSD_poll_res heatshrink_decoder_poll(heatshrink_decoder *hsd, uint8_t *out_buf, size_t out_buf_size, size_t *output_size); + +/* Notify the dencoder that the input stream is finished. + * If the return value is HSDR_FINISH_MORE, there is still more output, so + * call heatshrink_decoder_poll and repeat. */ +HSD_finish_res heatshrink_decoder_finish(heatshrink_decoder *hsd); diff --git a/Marlin/src/libs/hex_print.cpp b/Marlin/src/libs/hex_print.cpp new file mode 100644 index 0000000..0f746d6 --- /dev/null +++ b/Marlin/src/libs/hex_print.cpp @@ -0,0 +1,90 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../inc/MarlinConfigPre.h" + +#if NEED_HEX_PRINT + +#include "hex_print.h" +#include "../core/serial.h" + +#ifdef CPU_32_BIT + constexpr int byte_start = 4; + static char _hex[] = "0x00000000"; +#else + constexpr int byte_start = 0; + static char _hex[] = "0x0000"; +#endif + +char* hex_byte(const uint8_t b) { + _hex[byte_start + 4] = hex_nybble(b >> 4); + _hex[byte_start + 5] = hex_nybble(b); + return &_hex[byte_start + 4]; +} + +inline void _hex_word(const uint16_t w) { + _hex[byte_start + 2] = hex_nybble(w >> 12); + _hex[byte_start + 3] = hex_nybble(w >> 8); + _hex[byte_start + 4] = hex_nybble(w >> 4); + _hex[byte_start + 5] = hex_nybble(w); +} + +char* hex_word(const uint16_t w) { + _hex_word(w); + return &_hex[byte_start + 2]; +} + +#ifdef CPU_32_BIT + char* hex_long(const uintptr_t l) { + _hex[2] = hex_nybble(l >> 28); + _hex[3] = hex_nybble(l >> 24); + _hex[4] = hex_nybble(l >> 20); + _hex[5] = hex_nybble(l >> 16); + _hex_word((uint16_t)(l & 0xFFFF)); + return &_hex[2]; + } +#endif + +char* hex_address(const void * const w) { + #ifdef CPU_32_BIT + (void)hex_long((uintptr_t)w); + #else + (void)hex_word((uintptr_t)w); + #endif + return _hex; +} + +void print_hex_nybble(const uint8_t n) { SERIAL_CHAR(hex_nybble(n)); } +void print_hex_byte(const uint8_t b) { SERIAL_ECHO(hex_byte(b)); } +void print_hex_word(const uint16_t w) { SERIAL_ECHO(hex_word(w)); } +void print_hex_address(const void * const w) { SERIAL_ECHO(hex_address(w)); } + +void print_hex_long(const uint32_t w, const char delimiter) { + SERIAL_ECHOPGM("0x"); + for (int B = 24; B >= 8; B -= 8){ + print_hex_byte(w >> B); + SERIAL_CHAR(delimiter); + } + print_hex_byte(w); +} + +#endif // NEED_HEX_PRINT diff --git a/Marlin/src/libs/hex_print.h b/Marlin/src/libs/hex_print.h new file mode 100644 index 0000000..40baa15 --- /dev/null +++ b/Marlin/src/libs/hex_print.h @@ -0,0 +1,41 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include <stdint.h> + +// +// Utility functions to create and print hex strings as nybble, byte, and word. +// + +FORCE_INLINE char hex_nybble(const uint8_t n) { + return (n & 0xF) + ((n & 0xF) < 10 ? '0' : 'A' - 10); +} +char* hex_byte(const uint8_t b); +char* hex_word(const uint16_t w); +char* hex_address(const void * const w); + +void print_hex_nybble(const uint8_t n); +void print_hex_byte(const uint8_t b); +void print_hex_word(const uint16_t w); +void print_hex_address(const void * const w); +void print_hex_long(const uint32_t w, const char delimiter); diff --git a/Marlin/src/libs/least_squares_fit.cpp b/Marlin/src/libs/least_squares_fit.cpp new file mode 100644 index 0000000..c7593c0 --- /dev/null +++ b/Marlin/src/libs/least_squares_fit.cpp @@ -0,0 +1,69 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +/** + * Least Squares Best Fit by Roxy and Ed Williams + * + * This algorithm is high speed and has a very small code footprint. + * Its results are identical to both the Iterative Least-Squares published + * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE + * it saves roughly 10K of program memory. It also does not require all of + * coordinates to be present during the calculations. Each point can be + * probed and then discarded. + */ + +#include "../inc/MarlinConfig.h" + +#if NEED_LSF + +#include "least_squares_fit.h" + +#include <math.h> + +int finish_incremental_LSF(struct linear_fit_data *lsf) { + + const float N = lsf->N; + + if (N == 0.0) + return 1; + + lsf->xbar /= N; + lsf->ybar /= N; + lsf->zbar /= N; + lsf->x2bar = lsf->x2bar / N - sq(lsf->xbar); + lsf->y2bar = lsf->y2bar / N - sq(lsf->ybar); + lsf->z2bar = lsf->z2bar / N - sq(lsf->zbar); + lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar; + lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar; + lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar; + const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar); + + if (ABS(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy)) + return 1; + + lsf->A = (lsf->yzbar * lsf->xybar - lsf->xzbar * lsf->y2bar) / DD; + lsf->B = (lsf->xzbar * lsf->xybar - lsf->yzbar * lsf->x2bar) / DD; + lsf->D = -(lsf->zbar + lsf->A * lsf->xbar + lsf->B * lsf->ybar); + return 0; +} + +#endif // NEED_LSF diff --git a/Marlin/src/libs/least_squares_fit.h b/Marlin/src/libs/least_squares_fit.h new file mode 100644 index 0000000..44ca8af --- /dev/null +++ b/Marlin/src/libs/least_squares_fit.h @@ -0,0 +1,89 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +/** + * Incremental Least Squares Best Fit By Roxy and Ed Williams + * + * This algorithm is high speed and has a very small code footprint. + * Its results are identical to both the Iterative Least-Squares published + * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE + * it saves roughly 10K of program memory. And even better... the data + * fed into the algorithm does not need to all be present at the same time. + * A point can be probed and its values fed into the algorithm and then discarded. + */ + +#include "../inc/MarlinConfig.h" +#include <math.h> + +struct linear_fit_data { + float xbar, ybar, zbar, + x2bar, y2bar, z2bar, + xybar, xzbar, yzbar, + max_absx, max_absy, + A, B, D, N; +}; + +inline void incremental_LSF_reset(struct linear_fit_data *lsf) { + memset(lsf, 0, sizeof(linear_fit_data)); +} + +inline void incremental_WLSF(struct linear_fit_data *lsf, const float &x, const float &y, const float &z, const float &w) { + // weight each accumulator by factor w, including the "number" of samples + // (analogous to calling inc_LSF twice with same values to weight it by 2X) + const float wx = w * x, wy = w * y, wz = w * z; + lsf->xbar += wx; + lsf->ybar += wy; + lsf->zbar += wz; + lsf->x2bar += wx * x; + lsf->y2bar += wy * y; + lsf->z2bar += wz * z; + lsf->xybar += wx * y; + lsf->xzbar += wx * z; + lsf->yzbar += wy * z; + lsf->N += w; + lsf->max_absx = _MAX(ABS(wx), lsf->max_absx); + lsf->max_absy = _MAX(ABS(wy), lsf->max_absy); +} +inline void incremental_WLSF(struct linear_fit_data *lsf, const xy_pos_t &pos, const float &z, const float &w) { + incremental_WLSF(lsf, pos.x, pos.y, z, w); +} + +inline void incremental_LSF(struct linear_fit_data *lsf, const float &x, const float &y, const float &z) { + lsf->xbar += x; + lsf->ybar += y; + lsf->zbar += z; + lsf->x2bar += sq(x); + lsf->y2bar += sq(y); + lsf->z2bar += sq(z); + lsf->xybar += x * y; + lsf->xzbar += x * z; + lsf->yzbar += y * z; + lsf->max_absx = _MAX(ABS(x), lsf->max_absx); + lsf->max_absy = _MAX(ABS(y), lsf->max_absy); + lsf->N += 1.0; +} +inline void incremental_LSF(struct linear_fit_data *lsf, const xy_pos_t &pos, const float &z) { + incremental_LSF(lsf, pos.x, pos.y, z); +} + +int finish_incremental_LSF(struct linear_fit_data *); diff --git a/Marlin/src/libs/nozzle.cpp b/Marlin/src/libs/nozzle.cpp new file mode 100644 index 0000000..4277e8d --- /dev/null +++ b/Marlin/src/libs/nozzle.cpp @@ -0,0 +1,256 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../inc/MarlinConfig.h" + +#if EITHER(NOZZLE_CLEAN_FEATURE, NOZZLE_PARK_FEATURE) + +#include "nozzle.h" + +Nozzle nozzle; + +#include "../MarlinCore.h" +#include "../module/motion.h" + +#if NOZZLE_CLEAN_MIN_TEMP > 20 + #include "../module/temperature.h" +#endif + +#if ENABLED(NOZZLE_CLEAN_FEATURE) + + /** + * @brief Stroke clean pattern + * @details Wipes the nozzle back and forth in a linear movement + * + * @param start xyz_pos_t defining the starting point + * @param end xyz_pos_t defining the ending point + * @param strokes number of strokes to execute + */ + void Nozzle::stroke(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes) { + TERN_(NOZZLE_CLEAN_GOBACK, const xyz_pos_t oldpos = current_position); + + // Move to the starting point + #if ENABLED(NOZZLE_CLEAN_NO_Z) + #if ENABLED(NOZZLE_CLEAN_NO_Y) + do_blocking_move_to_x(start.x); + #else + do_blocking_move_to_xy(start); + #endif + #else + do_blocking_move_to(start); + #endif + + // Start the stroke pattern + LOOP_L_N(i, strokes >> 1) { + #if ENABLED(NOZZLE_CLEAN_NO_Y) + do_blocking_move_to_x(end.x); + do_blocking_move_to_x(start.x); + #else + do_blocking_move_to_xy(end); + do_blocking_move_to_xy(start); + #endif + } + + TERN_(NOZZLE_CLEAN_GOBACK, do_blocking_move_to(oldpos)); + } + + /** + * @brief Zig-zag clean pattern + * @details Apply a zig-zag cleaning pattern + * + * @param start xyz_pos_t defining the starting point + * @param end xyz_pos_t defining the ending point + * @param strokes number of strokes to execute + * @param objects number of triangles to do + */ + void Nozzle::zigzag(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes, const uint8_t &objects) { + const xy_pos_t diff = end - start; + if (!diff.x || !diff.y) return; + + TERN_(NOZZLE_CLEAN_GOBACK, const xyz_pos_t back = current_position); + + #if ENABLED(NOZZLE_CLEAN_NO_Z) + do_blocking_move_to_xy(start); + #else + do_blocking_move_to(start); + #endif + + const uint8_t zigs = objects << 1; + const bool horiz = ABS(diff.x) >= ABS(diff.y); // Do a horizontal wipe? + const float P = (horiz ? diff.x : diff.y) / zigs; // Period of each zig / zag + const xyz_pos_t *side; + LOOP_L_N(j, strokes) { + for (int8_t i = 0; i < zigs; i++) { + side = (i & 1) ? &end : &start; + if (horiz) + do_blocking_move_to_xy(start.x + i * P, side->y); + else + do_blocking_move_to_xy(side->x, start.y + i * P); + } + for (int8_t i = zigs; i >= 0; i--) { + side = (i & 1) ? &end : &start; + if (horiz) + do_blocking_move_to_xy(start.x + i * P, side->y); + else + do_blocking_move_to_xy(side->x, start.y + i * P); + } + } + + TERN_(NOZZLE_CLEAN_GOBACK, do_blocking_move_to(back)); + } + + /** + * @brief Circular clean pattern + * @details Apply a circular cleaning pattern + * + * @param start xyz_pos_t defining the middle of circle + * @param strokes number of strokes to execute + * @param radius radius of circle + */ + void Nozzle::circle(const xyz_pos_t &start, const xyz_pos_t &middle, const uint8_t &strokes, const float &radius) { + if (strokes == 0) return; + + TERN_(NOZZLE_CLEAN_GOBACK, const xyz_pos_t back = current_position); + TERN(NOZZLE_CLEAN_NO_Z, do_blocking_move_to_xy, do_blocking_move_to)(start); + + LOOP_L_N(s, strokes) + LOOP_L_N(i, NOZZLE_CLEAN_CIRCLE_FN) + do_blocking_move_to_xy( + middle.x + sin((RADIANS(360) / NOZZLE_CLEAN_CIRCLE_FN) * i) * radius, + middle.y + cos((RADIANS(360) / NOZZLE_CLEAN_CIRCLE_FN) * i) * radius + ); + + // Let's be safe + do_blocking_move_to_xy(start); + + TERN_(NOZZLE_CLEAN_GOBACK, do_blocking_move_to(back)); + } + + /** + * @brief Clean the nozzle + * @details Starts the selected clean procedure pattern + * + * @param pattern one of the available patterns + * @param argument depends on the cleaning pattern + */ + void Nozzle::clean(const uint8_t &pattern, const uint8_t &strokes, const float &radius, const uint8_t &objects, const uint8_t cleans) { + xyz_pos_t start[HOTENDS] = NOZZLE_CLEAN_START_POINT, end[HOTENDS] = NOZZLE_CLEAN_END_POINT, middle[HOTENDS] = NOZZLE_CLEAN_CIRCLE_MIDDLE; + + const uint8_t arrPos = ANY(SINGLENOZZLE, MIXING_EXTRUDER) ? 0 : active_extruder; + + #if NOZZLE_CLEAN_MIN_TEMP > 20 + if (thermalManager.degTargetHotend(arrPos) < NOZZLE_CLEAN_MIN_TEMP) { + #if ENABLED(NOZZLE_CLEAN_HEATUP) + SERIAL_ECHOLNPGM("Nozzle too Cold - Heating"); + thermalManager.setTargetHotend(NOZZLE_CLEAN_MIN_TEMP, arrPos); + thermalManager.wait_for_hotend(arrPos); + #else + SERIAL_ECHOLNPGM("Nozzle too cold - Skipping wipe"); + return; + #endif + } + #endif + + #if HAS_SOFTWARE_ENDSTOPS + + #define LIMIT_AXIS(A) do{ \ + LIMIT( start[arrPos].A, soft_endstop.min.A, soft_endstop.max.A); \ + LIMIT(middle[arrPos].A, soft_endstop.min.A, soft_endstop.max.A); \ + LIMIT( end[arrPos].A, soft_endstop.min.A, soft_endstop.max.A); \ + }while(0) + + if (soft_endstop.enabled()) { + + LIMIT_AXIS(x); + LIMIT_AXIS(y); + LIMIT_AXIS(z); + const bool radiusOutOfRange = (middle[arrPos].x + radius > soft_endstop.max.x) + || (middle[arrPos].x - radius < soft_endstop.min.x) + || (middle[arrPos].y + radius > soft_endstop.max.y) + || (middle[arrPos].y - radius < soft_endstop.min.y); + if (radiusOutOfRange && pattern == 2) { + SERIAL_ECHOLNPGM("Warning: Radius Out of Range"); + return; + } + + } + + #endif + + if (pattern == 2) { + if (!(cleans & (_BV(X_AXIS) | _BV(Y_AXIS)))) { + SERIAL_ECHOLNPGM("Warning: Clean Circle requires XY"); + return; + } + } + else { + if (!TEST(cleans, X_AXIS)) start[arrPos].x = end[arrPos].x = current_position.x; + if (!TEST(cleans, Y_AXIS)) start[arrPos].y = end[arrPos].y = current_position.y; + } + if (!TEST(cleans, Z_AXIS)) start[arrPos].z = end[arrPos].z = current_position.z; + + switch (pattern) { + case 1: zigzag(start[arrPos], end[arrPos], strokes, objects); break; + case 2: circle(start[arrPos], middle[arrPos], strokes, radius); break; + default: stroke(start[arrPos], end[arrPos], strokes); + } + } + +#endif // NOZZLE_CLEAN_FEATURE + +#if ENABLED(NOZZLE_PARK_FEATURE) + + void Nozzle::park(const uint8_t z_action, const xyz_pos_t &park/*=NOZZLE_PARK_POINT*/) { + constexpr feedRate_t fr_xy = NOZZLE_PARK_XY_FEEDRATE, fr_z = NOZZLE_PARK_Z_FEEDRATE; + + switch (z_action) { + case 1: // Go to Z-park height + do_blocking_move_to_z(park.z, fr_z); + break; + + case 2: // Raise by Z-park height + do_blocking_move_to_z(_MIN(current_position.z + park.z, Z_MAX_POS), fr_z); + break; + + default: { + // Apply a minimum raise, overriding G27 Z + const float min_raised_z =_MIN(Z_MAX_POS, current_position.z + #ifdef NOZZLE_PARK_Z_RAISE_MIN + + NOZZLE_PARK_Z_RAISE_MIN + #endif + ); + do_blocking_move_to_z(_MAX(park.z, min_raised_z), fr_z); + } break; + } + + do_blocking_move_to_xy( + TERN(NOZZLE_PARK_Y_ONLY, current_position, park).x, + TERN(NOZZLE_PARK_X_ONLY, current_position, park).y, + fr_xy + ); + + report_current_position(); + } + +#endif // NOZZLE_PARK_FEATURE + +#endif // NOZZLE_CLEAN_FEATURE || NOZZLE_PARK_FEATURE diff --git a/Marlin/src/libs/nozzle.h b/Marlin/src/libs/nozzle.h new file mode 100644 index 0000000..81594b1 --- /dev/null +++ b/Marlin/src/libs/nozzle.h @@ -0,0 +1,91 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../inc/MarlinConfig.h" + +/** + * @brief Nozzle class + * + * @todo: Do not ignore the end.z value and allow XYZ movements + */ +class Nozzle { + private: + + #if ENABLED(NOZZLE_CLEAN_FEATURE) + + /** + * @brief Stroke clean pattern + * @details Wipes the nozzle back and forth in a linear movement + * + * @param start xyz_pos_t defining the starting point + * @param end xyz_pos_t defining the ending point + * @param strokes number of strokes to execute + */ + static void stroke(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes) _Os; + + /** + * @brief Zig-zag clean pattern + * @details Apply a zig-zag cleaning pattern + * + * @param start xyz_pos_t defining the starting point + * @param end xyz_pos_t defining the ending point + * @param strokes number of strokes to execute + * @param objects number of objects to create + */ + static void zigzag(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes, const uint8_t &objects) _Os; + + /** + * @brief Circular clean pattern + * @details Apply a circular cleaning pattern + * + * @param start xyz_pos_t defining the middle of circle + * @param strokes number of strokes to execute + * @param radius radius of circle + */ + static void circle(const xyz_pos_t &start, const xyz_pos_t &middle, const uint8_t &strokes, const float &radius) _Os; + + #endif // NOZZLE_CLEAN_FEATURE + + public: + + #if ENABLED(NOZZLE_CLEAN_FEATURE) + + /** + * @brief Clean the nozzle + * @details Starts the selected clean procedure pattern + * + * @param pattern one of the available patterns + * @param argument depends on the cleaning pattern + */ + static void clean(const uint8_t &pattern, const uint8_t &strokes, const float &radius, const uint8_t &objects, const uint8_t cleans) _Os; + + #endif // NOZZLE_CLEAN_FEATURE + + #if ENABLED(NOZZLE_PARK_FEATURE) + + static void park(const uint8_t z_action, const xyz_pos_t &park=NOZZLE_PARK_POINT) _Os; + + #endif +}; + +extern Nozzle nozzle; diff --git a/Marlin/src/libs/numtostr.cpp b/Marlin/src/libs/numtostr.cpp new file mode 100644 index 0000000..90696e9 --- /dev/null +++ b/Marlin/src/libs/numtostr.cpp @@ -0,0 +1,408 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "numtostr.h" + +#include "../inc/MarlinConfigPre.h" +#include "../core/utility.h" + +char conv[8] = { 0 }; + +#define DIGIT(n) ('0' + (n)) +#define DIGIMOD(n, f) DIGIT((n)/(f) % 10) +#define RJDIGIT(n, f) ((n) >= (f) ? DIGIMOD(n, f) : ' ') +#define MINUSOR(n, alt) (n >= 0 ? (alt) : (n = -n, '-')) +#define INTFLOAT(V,N) (((V) * 10 * pow(10, N) + ((V) < 0 ? -5: 5)) / 10) // pow10? +#define UINTFLOAT(V,N) INTFLOAT((V) < 0 ? -(V) : (V), N) + +// Format uint8_t (0-100) as rj string with 123% / _12% / __1% format +const char* pcttostrpctrj(const uint8_t i) { + conv[3] = RJDIGIT(i, 100); + conv[4] = RJDIGIT(i, 10); + conv[5] = DIGIMOD(i, 1); + conv[6] = '%'; + return &conv[3]; +} + +// Convert uint8_t (0-255) to a percentage, format as above +const char* ui8tostr4pctrj(const uint8_t i) { + return pcttostrpctrj(ui8_to_percent(i)); +} + +// Convert unsigned 8bit int to string 123 format +const char* ui8tostr3rj(const uint8_t i) { + conv[4] = RJDIGIT(i, 100); + conv[5] = RJDIGIT(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[4]; +} + +// Convert uint8_t to string with 12 format +const char* ui8tostr2(const uint8_t i) { + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[5]; +} + +// Convert signed 8bit int to rj string with 123 or -12 format +const char* i8tostr3rj(const int8_t x) { + int xx = x; + conv[4] = MINUSOR(xx, RJDIGIT(xx, 100)); + conv[5] = RJDIGIT(xx, 10); + conv[6] = DIGIMOD(xx, 1); + return &conv[4]; +} + +#if HAS_PRINT_PROGRESS_PERMYRIAD + // Convert unsigned 16-bit permyriad to percent with 100 / 23 / 23.4 / 3.45 format + const char* permyriadtostr4(const uint16_t xx) { + if (xx >= 10000) + return "100"; + else if (xx >= 1000) { + conv[3] = DIGIMOD(xx, 1000); + conv[4] = DIGIMOD(xx, 100); + conv[5] = '.'; + conv[6] = DIGIMOD(xx, 10); + return &conv[3]; + } + else if (xx % 100 == 0) { + conv[4] = ' '; + conv[5] = RJDIGIT(xx, 1000); + conv[6] = DIGIMOD(xx, 100); + return &conv[4]; + } + else { + conv[3] = DIGIMOD(xx, 100); + conv[4] = '.'; + conv[5] = DIGIMOD(xx, 10); + conv[6] = RJDIGIT(xx, 1); + return &conv[3]; + } + } +#endif + +// Convert unsigned 16bit int to string 12345 format +const char* ui16tostr5rj(const uint16_t xx) { + conv[2] = RJDIGIT(xx, 10000); + conv[3] = RJDIGIT(xx, 1000); + conv[4] = RJDIGIT(xx, 100); + conv[5] = RJDIGIT(xx, 10); + conv[6] = DIGIMOD(xx, 1); + return &conv[2]; +} + +// Convert unsigned 16bit int to string 1234 format +const char* ui16tostr4rj(const uint16_t xx) { + conv[3] = RJDIGIT(xx, 1000); + conv[4] = RJDIGIT(xx, 100); + conv[5] = RJDIGIT(xx, 10); + conv[6] = DIGIMOD(xx, 1); + return &conv[3]; +} + +// Convert unsigned 16bit int to string 123 format +const char* ui16tostr3rj(const uint16_t xx) { + conv[4] = RJDIGIT(xx, 100); + conv[5] = RJDIGIT(xx, 10); + conv[6] = DIGIMOD(xx, 1); + return &conv[4]; +} + +// Convert signed 16bit int to rj string with 123 or -12 format +const char* i16tostr3rj(const int16_t x) { + int xx = x; + conv[4] = MINUSOR(xx, RJDIGIT(xx, 100)); + conv[5] = RJDIGIT(xx, 10); + conv[6] = DIGIMOD(xx, 1); + return &conv[4]; +} + +// Convert unsigned 16bit int to lj string with 123 format +const char* i16tostr3left(const int16_t i) { + char *str = &conv[6]; + *str = DIGIMOD(i, 1); + if (i >= 10) { + *(--str) = DIGIMOD(i, 10); + if (i >= 100) + *(--str) = DIGIMOD(i, 100); + } + return str; +} + +// Convert signed 16bit int to rj string with 1234, _123, -123, _-12, or __-1 format +const char* i16tostr4signrj(const int16_t i) { + const bool neg = i < 0; + const int ii = neg ? -i : i; + if (i >= 1000) { + conv[3] = DIGIMOD(ii, 1000); + conv[4] = DIGIMOD(ii, 100); + conv[5] = DIGIMOD(ii, 10); + } + else if (ii >= 100) { + conv[3] = neg ? '-' : ' '; + conv[4] = DIGIMOD(ii, 100); + conv[5] = DIGIMOD(ii, 10); + } + else { + conv[3] = ' '; + conv[4] = ' '; + if (ii >= 10) { + conv[4] = neg ? '-' : ' '; + conv[5] = DIGIMOD(ii, 10); + } + else { + conv[5] = neg ? '-' : ' '; + } + } + conv[6] = DIGIMOD(ii, 1); + return &conv[3]; +} + +// Convert unsigned float to string with 1.23 format +const char* ftostr12ns(const float &f) { + const long i = UINTFLOAT(f, 2); + conv[3] = DIGIMOD(i, 100); + conv[4] = '.'; + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[3]; +} + +// Convert unsigned float to string with 12.3 format +const char* ftostr31ns(const float &f) { + const long i = UINTFLOAT(f, 1); + conv[3] = DIGIMOD(i, 100); + conv[4] = DIGIMOD(i, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(i, 1); + return &conv[3]; +} + +// Convert unsigned float to string with 123.4 format +const char* ftostr41ns(const float &f) { + const long i = UINTFLOAT(f, 1); + conv[2] = DIGIMOD(i, 1000); + conv[3] = DIGIMOD(i, 100); + conv[4] = DIGIMOD(i, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(i, 1); + return &conv[2]; +} + +// Convert signed float to fixed-length string with 12.34 / _2.34 / -2.34 or -23.45 / 123.45 format +const char* ftostr42_52(const float &f) { + if (f <= -10 || f >= 100) return ftostr52(f); // -23.45 / 123.45 + long i = INTFLOAT(f, 2); + conv[2] = (f >= 0 && f < 10) ? ' ' : MINUSOR(i, DIGIMOD(i, 1000)); + conv[3] = DIGIMOD(i, 100); + conv[4] = '.'; + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[2]; +} + +// Convert signed float to fixed-length string with 023.45 / -23.45 format +const char* ftostr52(const float &f) { + long i = INTFLOAT(f, 2); + conv[1] = MINUSOR(i, DIGIMOD(i, 10000)); + conv[2] = DIGIMOD(i, 1000); + conv[3] = DIGIMOD(i, 100); + conv[4] = '.'; + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[1]; +} + +// Convert signed float to fixed-length string with 12.345 / _2.345 / -2.345 or -23.45 / 123.45 format +const char* ftostr53_63(const float &f) { + if (f <= -10 || f >= 100) return ftostr63(f); // -23.456 / 123.456 + long i = INTFLOAT(f, 3); + conv[1] = (f >= 0 && f < 10) ? ' ' : MINUSOR(i, DIGIMOD(i, 10000)); + conv[2] = DIGIMOD(i, 1000); + conv[3] = '.'; + conv[4] = DIGIMOD(i, 100); + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[1]; +} + +// Convert signed float to fixed-length string with 023.456 / -23.456 format +const char* ftostr63(const float &f) { + long i = INTFLOAT(f, 3); + conv[0] = MINUSOR(i, DIGIMOD(i, 100000)); + conv[1] = DIGIMOD(i, 10000); + conv[2] = DIGIMOD(i, 1000); + conv[3] = '.'; + conv[4] = DIGIMOD(i, 100); + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[0]; +} + +#if ENABLED(LCD_DECIMAL_SMALL_XY) + + // Convert float to rj string with 1234, _123, -123, _-12, 12.3, _1.2, or -1.2 format + const char* ftostr4sign(const float &f) { + const int i = INTFLOAT(f, 1); + if (!WITHIN(i, -99, 999)) return i16tostr4signrj((int)f); + const bool neg = i < 0; + const int ii = neg ? -i : i; + conv[3] = neg ? '-' : (ii >= 100 ? DIGIMOD(ii, 100) : ' '); + conv[4] = DIGIMOD(ii, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(ii, 1); + return &conv[3]; + } + +#endif + +// Convert float to fixed-length string with +12.3 / -12.3 format +const char* ftostr31sign(const float &f) { + int i = INTFLOAT(f, 1); + conv[2] = MINUSOR(i, '+'); + conv[3] = DIGIMOD(i, 100); + conv[4] = DIGIMOD(i, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(i, 1); + return &conv[2]; +} + +// Convert float to fixed-length string with +123.4 / -123.4 format +const char* ftostr41sign(const float &f) { + int i = INTFLOAT(f, 1); + conv[1] = MINUSOR(i, '+'); + conv[2] = DIGIMOD(i, 1000); + conv[3] = DIGIMOD(i, 100); + conv[4] = DIGIMOD(i, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(i, 1); + return &conv[1]; +} + +// Convert signed float to string (6 digit) with -1.234 / _0.000 / +1.234 format +const char* ftostr43sign(const float &f, char plus/*=' '*/) { + long i = INTFLOAT(f, 3); + conv[1] = i ? MINUSOR(i, plus) : ' '; + conv[2] = DIGIMOD(i, 1000); + conv[3] = '.'; + conv[4] = DIGIMOD(i, 100); + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[1]; +} + +// Convert signed float to string (5 digit) with -1.2345 / _0.0000 / +1.2345 format +const char* ftostr54sign(const float &f, char plus/*=' '*/) { + long i = INTFLOAT(f, 4); + conv[0] = i ? MINUSOR(i, plus) : ' '; + conv[1] = DIGIMOD(i, 10000); + conv[2] = '.'; + conv[3] = DIGIMOD(i, 1000); + conv[4] = DIGIMOD(i, 100); + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return &conv[0]; +} + +// Convert unsigned float to rj string with 12345 format +const char* ftostr5rj(const float &f) { + const long i = UINTFLOAT(f, 0); + return ui16tostr5rj(i); +} + +// Convert signed float to string with +1234.5 format +const char* ftostr51sign(const float &f) { + long i = INTFLOAT(f, 1); + conv[0] = MINUSOR(i, '+'); + conv[1] = DIGIMOD(i, 10000); + conv[2] = DIGIMOD(i, 1000); + conv[3] = DIGIMOD(i, 100); + conv[4] = DIGIMOD(i, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(i, 1); + return conv; +} + +// Convert signed float to string with +123.45 format +const char* ftostr52sign(const float &f) { + long i = INTFLOAT(f, 2); + conv[0] = MINUSOR(i, '+'); + conv[1] = DIGIMOD(i, 10000); + conv[2] = DIGIMOD(i, 1000); + conv[3] = DIGIMOD(i, 100); + conv[4] = '.'; + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return conv; +} + +// Convert signed float to string with +12.345 format +const char* ftostr53sign(const float &f) { + long i = INTFLOAT(f, 3); + conv[0] = MINUSOR(i, '+'); + conv[1] = DIGIMOD(i, 10000); + conv[2] = DIGIMOD(i, 1000); + conv[3] = '.'; + conv[4] = DIGIMOD(i, 100); + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIMOD(i, 1); + return conv; +} + +// Convert unsigned float to string with ____4.5, __34.5, _234.5, 1234.5 format +const char* ftostr51rj(const float &f) { + const long i = UINTFLOAT(f, 1); + conv[0] = ' '; + conv[1] = RJDIGIT(i, 10000); + conv[2] = RJDIGIT(i, 1000); + conv[3] = RJDIGIT(i, 100); + conv[4] = DIGIMOD(i, 10); + conv[5] = '.'; + conv[6] = DIGIMOD(i, 1); + return conv; +} + +// Convert signed float to space-padded string with -_23.4_ format +const char* ftostr52sp(const float &f) { + long i = INTFLOAT(f, 2); + uint8_t dig; + conv[0] = MINUSOR(i, ' '); + conv[1] = RJDIGIT(i, 10000); + conv[2] = RJDIGIT(i, 1000); + conv[3] = DIGIMOD(i, 100); + + if ((dig = i % 10)) { // second digit after decimal point? + conv[4] = '.'; + conv[5] = DIGIMOD(i, 10); + conv[6] = DIGIT(dig); + } + else { + if ((dig = (i / 10) % 10)) { // first digit after decimal point? + conv[4] = '.'; + conv[5] = DIGIT(dig); + } + else // nothing after decimal point + conv[4] = conv[5] = ' '; + conv[6] = ' '; + } + return conv; +} diff --git a/Marlin/src/libs/numtostr.h b/Marlin/src/libs/numtostr.h new file mode 100644 index 0000000..40c298a --- /dev/null +++ b/Marlin/src/libs/numtostr.h @@ -0,0 +1,128 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include <stdint.h> + +// Format uint8_t (0-100) as rj string with 123% / _12% / __1% format +const char* pcttostrpctrj(const uint8_t i); + +// Convert uint8_t (0-255) to a percentage, format as above +const char* ui8tostr4pctrj(const uint8_t i); + +// Convert uint8_t to string with 12 format +const char* ui8tostr2(const uint8_t x); + +// Convert uint8_t to string with 123 format +const char* ui8tostr3rj(const uint8_t i); + +// Convert int8_t to string with 123 format +const char* i8tostr3rj(const int8_t x); + +#if HAS_PRINT_PROGRESS_PERMYRIAD + // Convert 16-bit unsigned permyriad value to percent: 100 / 23 / 23.4 / 3.45 + const char* permyriadtostr4(const uint16_t xx); +#endif + +// Convert uint16_t to string with 12345 format +const char* ui16tostr5rj(const uint16_t x); + +// Convert uint16_t to string with 1234 format +const char* ui16tostr4rj(const uint16_t x); + +// Convert uint16_t to string with 123 format +const char* ui16tostr3rj(const uint16_t x); + +// Convert int16_t to string with 123 format +const char* i16tostr3rj(const int16_t x); + +// Convert unsigned int to lj string with 123 format +const char* i16tostr3left(const int16_t xx); + +// Convert signed int to rj string with _123, -123, _-12, or __-1 format +const char* i16tostr4signrj(const int16_t x); + +// Convert unsigned float to string with 1.23 format +const char* ftostr12ns(const float &x); + +// Convert unsigned float to string with 12.3 format +const char* ftostr31ns(const float &x); + +// Convert unsigned float to string with 123.4 format +const char* ftostr41ns(const float &x); + +// Convert signed float to fixed-length string with 12.34 / _2.34 / -2.34 or -23.45 / 123.45 format +const char* ftostr42_52(const float &x); + +// Convert signed float to fixed-length string with 023.45 / -23.45 format +const char* ftostr52(const float &x); + +// Convert signed float to fixed-length string with 12.345 / -2.345 or 023.456 / -23.456 format +const char* ftostr53_63(const float &x); + +// Convert signed float to fixed-length string with 023.456 / -23.456 format +const char* ftostr63(const float &x); + +// Convert float to fixed-length string with +12.3 / -12.3 format +const char* ftostr31sign(const float &x); + +// Convert float to fixed-length string with +123.4 / -123.4 format +const char* ftostr41sign(const float &x); + +// Convert signed float to string (6 digit) with -1.234 / _0.000 / +1.234 format +const char* ftostr43sign(const float &x, char plus=' '); + +// Convert signed float to string (5 digit) with -1.2345 / _0.0000 / +1.2345 format +const char* ftostr54sign(const float &x, char plus=' '); + +// Convert unsigned float to rj string with 12345 format +const char* ftostr5rj(const float &x); + +// Convert signed float to string with +1234.5 format +const char* ftostr51sign(const float &x); + +// Convert signed float to space-padded string with -_23.4_ format +const char* ftostr52sp(const float &x); + +// Convert signed float to string with +123.45 format +const char* ftostr52sign(const float &x); + +// Convert signed float to string with +12.345 format +const char* ftostr53sign(const float &f); + +// Convert unsigned float to string with 1234.5 format omitting trailing zeros +const char* ftostr51rj(const float &x); + +#include "../core/macros.h" + +// Convert float to rj string with 123 or -12 format +FORCE_INLINE const char* ftostr3(const float &x) { return i16tostr3rj(int16_t(x + (x < 0 ? -0.5f : 0.5f))); } + +#include "../inc/MarlinConfigPre.h" + +#if ENABLED(LCD_DECIMAL_SMALL_XY) + // Convert float to rj string with 1234, _123, 12.3, _1.2, -123, _-12, or -1.2 format + const char* ftostr4sign(const float &fx); +#else + // Convert float to rj string with 1234, _123, -123, __12, _-12, ___1, or __-1 format + FORCE_INLINE const char* ftostr4sign(const float &x) { return i16tostr4signrj(int16_t(x + (x < 0 ? -0.5f : 0.5f))); } +#endif diff --git a/Marlin/src/libs/private_spi.h b/Marlin/src/libs/private_spi.h new file mode 100644 index 0000000..1d8eacd --- /dev/null +++ b/Marlin/src/libs/private_spi.h @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "softspi.h" +#include <stdint.h> + +template<uint8_t MisoPin, uint8_t MosiPin, uint8_t SckPin> +class SPIclass { + static SoftSPI<MisoPin, MosiPin, SckPin> softSPI; + public: + FORCE_INLINE static void init() { softSPI.begin(); } + FORCE_INLINE static void send(uint8_t data) { softSPI.send(data); } + FORCE_INLINE static uint8_t receive() { return softSPI.receive(); } +}; + +// Hardware SPI +template<> +class SPIclass<SD_MISO_PIN, SD_MOSI_PIN, SD_SCK_PIN> { + public: + FORCE_INLINE static void init() { + OUT_WRITE(SD_SCK_PIN, LOW); + OUT_WRITE(SD_MOSI_PIN, HIGH); + SET_INPUT_PULLUP(SD_MISO_PIN); + } + FORCE_INLINE static uint8_t receive() { + #if defined(__AVR__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__IMXRT1062__) + SPDR = 0; + for (;!TEST(SPSR, SPIF);); + return SPDR; + #else + return spiRec(); + #endif + } +}; diff --git a/Marlin/src/libs/softspi.h b/Marlin/src/libs/softspi.h new file mode 100644 index 0000000..5d48f9f --- /dev/null +++ b/Marlin/src/libs/softspi.h @@ -0,0 +1,746 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +// +// Based on https://github.com/niteris/ArduinoSoftSpi +// + +#include "../HAL/shared/Marduino.h" + +#ifndef FORCE_INLINE + #define FORCE_INLINE inline __attribute__((always_inline)) +#endif + +#define nop __asm__ volatile ("nop") // NOP for timing + +#ifdef __arm__ + + #ifdef CORE_TEENSY + + /** + * Read pin value + * @param[in] pin Arduino pin number + * @return value read + */ + FORCE_INLINE static bool fastDigitalRead(uint8_t pin) { + return *portInputRegister(pin); + } + + /** + * Set pin value + * @param[in] pin Arduino pin number + * @param[in] level value to write + */ + FORCE_INLINE static void fastDigitalWrite(uint8_t pin, bool value) { + if (value) + *portSetRegister(pin) = 1; + else + *portClearRegister(pin) = 1; + } + + #else // !CORE_TEENSY + + /** + * Read pin value + * @param[in] pin Arduino pin number + * @return value read + */ + FORCE_INLINE static bool fastDigitalRead(uint8_t pin) { + return digitalRead(pin); + } + + /** + * Set pin value + * @param[in] pin Arduino pin number + * @param[in] level value to write + */ + FORCE_INLINE static void fastDigitalWrite(uint8_t pin, bool value) { + digitalWrite(pin, value); + } + + #endif // !CORE_TEENSY + + inline void fastDigitalToggle(uint8_t pin) { fastDigitalWrite(pin, !fastDigitalRead(pin)); } + + inline void fastPinMode(uint8_t pin, bool mode) { pinMode(pin, mode); } + +#else // !__arm__ + + #include <avr/io.h> + #include <util/atomic.h> + + /** + * @class pin_map_t + * @brief struct for mapping digital pins + */ + struct pin_map_t { + volatile uint8_t* ddr; /**< address of DDR for this pin */ + volatile uint8_t* pin; /**< address of PIN for this pin */ + volatile uint8_t* port; /**< address of PORT for this pin */ + uint8_t bit; /**< bit number for this pin */ + }; + + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || defined(__AVR_ATmega328P__) + + // 168 and 328 Arduinos + const static pin_map_t pinMap[] = { + {&DDRD, &PIND, &PORTD, 0}, // D0 0 + {&DDRD, &PIND, &PORTD, 1}, // D1 1 + {&DDRD, &PIND, &PORTD, 2}, // D2 2 + {&DDRD, &PIND, &PORTD, 3}, // D3 3 + {&DDRD, &PIND, &PORTD, 4}, // D4 4 + {&DDRD, &PIND, &PORTD, 5}, // D5 5 + {&DDRD, &PIND, &PORTD, 6}, // D6 6 + {&DDRD, &PIND, &PORTD, 7}, // D7 7 + {&DDRB, &PINB, &PORTB, 0}, // B0 8 + {&DDRB, &PINB, &PORTB, 1}, // B1 9 + {&DDRB, &PINB, &PORTB, 2}, // B2 10 + {&DDRB, &PINB, &PORTB, 3}, // B3 11 + {&DDRB, &PINB, &PORTB, 4}, // B4 12 + {&DDRB, &PINB, &PORTB, 5}, // B5 13 + {&DDRC, &PINC, &PORTC, 0}, // C0 14 + {&DDRC, &PINC, &PORTC, 1}, // C1 15 + {&DDRC, &PINC, &PORTC, 2}, // C2 16 + {&DDRC, &PINC, &PORTC, 3}, // C3 17 + {&DDRC, &PINC, &PORTC, 4}, // C4 18 + {&DDRC, &PINC, &PORTC, 5} // C5 19 + }; + + #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + + // Mega + static const pin_map_t pinMap[] = { + {&DDRE, &PINE, &PORTE, 0}, // E0 0 + {&DDRE, &PINE, &PORTE, 1}, // E1 1 + {&DDRE, &PINE, &PORTE, 4}, // E4 2 + {&DDRE, &PINE, &PORTE, 5}, // E5 3 + {&DDRG, &PING, &PORTG, 5}, // G5 4 + {&DDRE, &PINE, &PORTE, 3}, // E3 5 + {&DDRH, &PINH, &PORTH, 3}, // H3 6 + {&DDRH, &PINH, &PORTH, 4}, // H4 7 + {&DDRH, &PINH, &PORTH, 5}, // H5 8 + {&DDRH, &PINH, &PORTH, 6}, // H6 9 + {&DDRB, &PINB, &PORTB, 4}, // B4 10 + {&DDRB, &PINB, &PORTB, 5}, // B5 11 + {&DDRB, &PINB, &PORTB, 6}, // B6 12 + {&DDRB, &PINB, &PORTB, 7}, // B7 13 + {&DDRJ, &PINJ, &PORTJ, 1}, // J1 14 + {&DDRJ, &PINJ, &PORTJ, 0}, // J0 15 + {&DDRH, &PINH, &PORTH, 1}, // H1 16 + {&DDRH, &PINH, &PORTH, 0}, // H0 17 + {&DDRD, &PIND, &PORTD, 3}, // D3 18 + {&DDRD, &PIND, &PORTD, 2}, // D2 19 + {&DDRD, &PIND, &PORTD, 1}, // D1 20 + {&DDRD, &PIND, &PORTD, 0}, // D0 21 + {&DDRA, &PINA, &PORTA, 0}, // A0 22 + {&DDRA, &PINA, &PORTA, 1}, // A1 23 + {&DDRA, &PINA, &PORTA, 2}, // A2 24 + {&DDRA, &PINA, &PORTA, 3}, // A3 25 + {&DDRA, &PINA, &PORTA, 4}, // A4 26 + {&DDRA, &PINA, &PORTA, 5}, // A5 27 + {&DDRA, &PINA, &PORTA, 6}, // A6 28 + {&DDRA, &PINA, &PORTA, 7}, // A7 29 + {&DDRC, &PINC, &PORTC, 7}, // C7 30 + {&DDRC, &PINC, &PORTC, 6}, // C6 31 + {&DDRC, &PINC, &PORTC, 5}, // C5 32 + {&DDRC, &PINC, &PORTC, 4}, // C4 33 + {&DDRC, &PINC, &PORTC, 3}, // C3 34 + {&DDRC, &PINC, &PORTC, 2}, // C2 35 + {&DDRC, &PINC, &PORTC, 1}, // C1 36 + {&DDRC, &PINC, &PORTC, 0}, // C0 37 + {&DDRD, &PIND, &PORTD, 7}, // D7 38 + {&DDRG, &PING, &PORTG, 2}, // G2 39 + {&DDRG, &PING, &PORTG, 1}, // G1 40 + {&DDRG, &PING, &PORTG, 0}, // G0 41 + {&DDRL, &PINL, &PORTL, 7}, // L7 42 + {&DDRL, &PINL, &PORTL, 6}, // L6 43 + {&DDRL, &PINL, &PORTL, 5}, // L5 44 + {&DDRL, &PINL, &PORTL, 4}, // L4 45 + {&DDRL, &PINL, &PORTL, 3}, // L3 46 + {&DDRL, &PINL, &PORTL, 2}, // L2 47 + {&DDRL, &PINL, &PORTL, 1}, // L1 48 + {&DDRL, &PINL, &PORTL, 0}, // L0 49 + {&DDRB, &PINB, &PORTB, 3}, // B3 50 + {&DDRB, &PINB, &PORTB, 2}, // B2 51 + {&DDRB, &PINB, &PORTB, 1}, // B1 52 + {&DDRB, &PINB, &PORTB, 0}, // B0 53 + {&DDRF, &PINF, &PORTF, 0}, // F0 54 + {&DDRF, &PINF, &PORTF, 1}, // F1 55 + {&DDRF, &PINF, &PORTF, 2}, // F2 56 + {&DDRF, &PINF, &PORTF, 3}, // F3 57 + {&DDRF, &PINF, &PORTF, 4}, // F4 58 + {&DDRF, &PINF, &PORTF, 5}, // F5 59 + {&DDRF, &PINF, &PORTF, 6}, // F6 60 + {&DDRF, &PINF, &PORTF, 7}, // F7 61 + {&DDRK, &PINK, &PORTK, 0}, // K0 62 + {&DDRK, &PINK, &PORTK, 1}, // K1 63 + {&DDRK, &PINK, &PORTK, 2}, // K2 64 + {&DDRK, &PINK, &PORTK, 3}, // K3 65 + {&DDRK, &PINK, &PORTK, 4}, // K4 66 + {&DDRK, &PINK, &PORTK, 5}, // K5 67 + {&DDRK, &PINK, &PORTK, 6}, // K6 68 + {&DDRK, &PINK, &PORTK, 7}, // K7 69 + + // pins_MIGHTYBOARD_REVE.h + {&DDRG, &PING, &PORTG, 4}, // G4 70 + {&DDRG, &PING, &PORTG, 3}, // G3 71 + {&DDRJ, &PINJ, &PORTJ, 2}, // J2 72 + {&DDRJ, &PINJ, &PORTJ, 3}, // J3 73 + {&DDRJ, &PINJ, &PORTJ, 7}, // J7 74 + {&DDRJ, &PINJ, &PORTJ, 4}, // J4 75 + {&DDRJ, &PINJ, &PORTJ, 5}, // J5 76 + {&DDRJ, &PINJ, &PORTJ, 6}, // J6 77 + {&DDRE, &PINE, &PORTE, 2}, // E2 78 + {&DDRE, &PINE, &PORTE, 6} // E6 79 + }; + + #elif defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega1284__) \ + || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) \ + || defined(__AVR_ATmega64__) || defined(__AVR_ATmega32__) \ + || defined(__AVR_ATmega324__) || defined(__AVR_ATmega16__) + + #ifdef VARIANT_MIGHTY + + // Mighty Layout + static const pin_map_t pinMap[] = { + {&DDRB, &PINB, &PORTB, 0}, // B0 0 + {&DDRB, &PINB, &PORTB, 1}, // B1 1 + {&DDRB, &PINB, &PORTB, 2}, // B2 2 + {&DDRB, &PINB, &PORTB, 3}, // B3 3 + {&DDRB, &PINB, &PORTB, 4}, // B4 4 + {&DDRB, &PINB, &PORTB, 5}, // B5 5 + {&DDRB, &PINB, &PORTB, 6}, // B6 6 + {&DDRB, &PINB, &PORTB, 7}, // B7 7 + {&DDRD, &PIND, &PORTD, 0}, // D0 8 + {&DDRD, &PIND, &PORTD, 1}, // D1 9 + {&DDRD, &PIND, &PORTD, 2}, // D2 10 + {&DDRD, &PIND, &PORTD, 3}, // D3 11 + {&DDRD, &PIND, &PORTD, 4}, // D4 12 + {&DDRD, &PIND, &PORTD, 5}, // D5 13 + {&DDRD, &PIND, &PORTD, 6}, // D6 14 + {&DDRD, &PIND, &PORTD, 7}, // D7 15 + {&DDRC, &PINC, &PORTC, 0}, // C0 16 + {&DDRC, &PINC, &PORTC, 1}, // C1 17 + {&DDRC, &PINC, &PORTC, 2}, // C2 18 + {&DDRC, &PINC, &PORTC, 3}, // C3 19 + {&DDRC, &PINC, &PORTC, 4}, // C4 20 + {&DDRC, &PINC, &PORTC, 5}, // C5 21 + {&DDRC, &PINC, &PORTC, 6}, // C6 22 + {&DDRC, &PINC, &PORTC, 7}, // C7 23 + {&DDRA, &PINA, &PORTA, 0}, // A0 24 + {&DDRA, &PINA, &PORTA, 1}, // A1 25 + {&DDRA, &PINA, &PORTA, 2}, // A2 26 + {&DDRA, &PINA, &PORTA, 3}, // A3 27 + {&DDRA, &PINA, &PORTA, 4}, // A4 28 + {&DDRA, &PINA, &PORTA, 5}, // A5 29 + {&DDRA, &PINA, &PORTA, 6}, // A6 30 + {&DDRA, &PINA, &PORTA, 7} // A7 31 + }; + + #elif defined(VARIANT_BOBUINO) + + // Bobuino Layout + static const pin_map_t pinMap[] = { + {&DDRD, &PIND, &PORTD, 0}, // D0 0 + {&DDRD, &PIND, &PORTD, 1}, // D1 1 + {&DDRD, &PIND, &PORTD, 2}, // D2 2 + {&DDRD, &PIND, &PORTD, 3}, // D3 3 + {&DDRB, &PINB, &PORTB, 0}, // B0 4 + {&DDRB, &PINB, &PORTB, 1}, // B1 5 + {&DDRB, &PINB, &PORTB, 2}, // B2 6 + {&DDRB, &PINB, &PORTB, 3}, // B3 7 + {&DDRD, &PIND, &PORTD, 5}, // D5 8 + {&DDRD, &PIND, &PORTD, 6}, // D6 9 + {&DDRB, &PINB, &PORTB, 4}, // B4 10 + {&DDRB, &PINB, &PORTB, 5}, // B5 11 + {&DDRB, &PINB, &PORTB, 6}, // B6 12 + {&DDRB, &PINB, &PORTB, 7}, // B7 13 + {&DDRA, &PINA, &PORTA, 7}, // A7 14 + {&DDRA, &PINA, &PORTA, 6}, // A6 15 + {&DDRA, &PINA, &PORTA, 5}, // A5 16 + {&DDRA, &PINA, &PORTA, 4}, // A4 17 + {&DDRA, &PINA, &PORTA, 3}, // A3 18 + {&DDRA, &PINA, &PORTA, 2}, // A2 19 + {&DDRA, &PINA, &PORTA, 1}, // A1 20 + {&DDRA, &PINA, &PORTA, 0}, // A0 21 + {&DDRC, &PINC, &PORTC, 0}, // C0 22 + {&DDRC, &PINC, &PORTC, 1}, // C1 23 + {&DDRC, &PINC, &PORTC, 2}, // C2 24 + {&DDRC, &PINC, &PORTC, 3}, // C3 25 + {&DDRC, &PINC, &PORTC, 4}, // C4 26 + {&DDRC, &PINC, &PORTC, 5}, // C5 27 + {&DDRC, &PINC, &PORTC, 6}, // C6 28 + {&DDRC, &PINC, &PORTC, 7}, // C7 29 + {&DDRD, &PIND, &PORTD, 4}, // D4 30 + {&DDRD, &PIND, &PORTD, 7} // D7 31 + }; + + #elif defined(VARIANT_STANDARD) + + // Standard Layout + static const pin_map_t pinMap[] = { + {&DDRB, &PINB, &PORTB, 0}, // B0 0 + {&DDRB, &PINB, &PORTB, 1}, // B1 1 + {&DDRB, &PINB, &PORTB, 2}, // B2 2 + {&DDRB, &PINB, &PORTB, 3}, // B3 3 + {&DDRB, &PINB, &PORTB, 4}, // B4 4 + {&DDRB, &PINB, &PORTB, 5}, // B5 5 + {&DDRB, &PINB, &PORTB, 6}, // B6 6 + {&DDRB, &PINB, &PORTB, 7}, // B7 7 + {&DDRD, &PIND, &PORTD, 0}, // D0 8 + {&DDRD, &PIND, &PORTD, 1}, // D1 9 + {&DDRD, &PIND, &PORTD, 2}, // D2 10 + {&DDRD, &PIND, &PORTD, 3}, // D3 11 + {&DDRD, &PIND, &PORTD, 4}, // D4 12 + {&DDRD, &PIND, &PORTD, 5}, // D5 13 + {&DDRD, &PIND, &PORTD, 6}, // D6 14 + {&DDRD, &PIND, &PORTD, 7}, // D7 15 + {&DDRC, &PINC, &PORTC, 0}, // C0 16 + {&DDRC, &PINC, &PORTC, 1}, // C1 17 + {&DDRC, &PINC, &PORTC, 2}, // C2 18 + {&DDRC, &PINC, &PORTC, 3}, // C3 19 + {&DDRC, &PINC, &PORTC, 4}, // C4 20 + {&DDRC, &PINC, &PORTC, 5}, // C5 21 + {&DDRC, &PINC, &PORTC, 6}, // C6 22 + {&DDRC, &PINC, &PORTC, 7}, // C7 23 + {&DDRA, &PINA, &PORTA, 7}, // A7 24 + {&DDRA, &PINA, &PORTA, 6}, // A6 25 + {&DDRA, &PINA, &PORTA, 5}, // A5 26 + {&DDRA, &PINA, &PORTA, 4}, // A4 27 + {&DDRA, &PINA, &PORTA, 3}, // A3 28 + {&DDRA, &PINA, &PORTA, 2}, // A2 29 + {&DDRA, &PINA, &PORTA, 1}, // A1 30 + {&DDRA, &PINA, &PORTA, 0} // A0 31 + }; + + #else // !(VARIANT_MIGHTY || VARIANT_BOBUINO || VARIANT_STANDARD) + + #error Undefined variant 1284, 644, 324, 64, 32 + + #endif + + #elif defined(__AVR_ATmega32U4__) + + #ifdef CORE_TEENSY + + // Teensy 2.0 + static const pin_map_t pinMap[] = { + {&DDRB, &PINB, &PORTB, 0}, // B0 0 + {&DDRB, &PINB, &PORTB, 1}, // B1 1 + {&DDRB, &PINB, &PORTB, 2}, // B2 2 + {&DDRB, &PINB, &PORTB, 3}, // B3 3 + {&DDRB, &PINB, &PORTB, 7}, // B7 4 + {&DDRD, &PIND, &PORTD, 0}, // D0 5 + {&DDRD, &PIND, &PORTD, 1}, // D1 6 + {&DDRD, &PIND, &PORTD, 2}, // D2 7 + {&DDRD, &PIND, &PORTD, 3}, // D3 8 + {&DDRC, &PINC, &PORTC, 6}, // C6 9 + {&DDRC, &PINC, &PORTC, 7}, // C7 10 + {&DDRD, &PIND, &PORTD, 6}, // D6 11 + {&DDRD, &PIND, &PORTD, 7}, // D7 12 + {&DDRB, &PINB, &PORTB, 4}, // B4 13 + {&DDRB, &PINB, &PORTB, 5}, // B5 14 + {&DDRB, &PINB, &PORTB, 6}, // B6 15 + {&DDRF, &PINF, &PORTF, 7}, // F7 16 + {&DDRF, &PINF, &PORTF, 6}, // F6 17 + {&DDRF, &PINF, &PORTF, 5}, // F5 18 + {&DDRF, &PINF, &PORTF, 4}, // F4 19 + {&DDRF, &PINF, &PORTF, 1}, // F1 20 + {&DDRF, &PINF, &PORTF, 0}, // F0 21 + {&DDRD, &PIND, &PORTD, 4}, // D4 22 + {&DDRD, &PIND, &PORTD, 5}, // D5 23 + {&DDRE, &PINE, &PORTE, 6} // E6 24 + }; + + #else // !CORE_TEENSY + + // Leonardo + static const pin_map_t pinMap[] = { + {&DDRD, &PIND, &PORTD, 2}, // D2 0 + {&DDRD, &PIND, &PORTD, 3}, // D3 1 + {&DDRD, &PIND, &PORTD, 1}, // D1 2 + {&DDRD, &PIND, &PORTD, 0}, // D0 3 + {&DDRD, &PIND, &PORTD, 4}, // D4 4 + {&DDRC, &PINC, &PORTC, 6}, // C6 5 + {&DDRD, &PIND, &PORTD, 7}, // D7 6 + {&DDRE, &PINE, &PORTE, 6}, // E6 7 + {&DDRB, &PINB, &PORTB, 4}, // B4 8 + {&DDRB, &PINB, &PORTB, 5}, // B5 9 + {&DDRB, &PINB, &PORTB, 6}, // B6 10 + {&DDRB, &PINB, &PORTB, 7}, // B7 11 + {&DDRD, &PIND, &PORTD, 6}, // D6 12 + {&DDRC, &PINC, &PORTC, 7}, // C7 13 + {&DDRB, &PINB, &PORTB, 3}, // B3 14 + {&DDRB, &PINB, &PORTB, 1}, // B1 15 + {&DDRB, &PINB, &PORTB, 2}, // B2 16 + {&DDRB, &PINB, &PORTB, 0}, // B0 17 + {&DDRF, &PINF, &PORTF, 7}, // F7 18 + {&DDRF, &PINF, &PORTF, 6}, // F6 19 + {&DDRF, &PINF, &PORTF, 5}, // F5 20 + {&DDRF, &PINF, &PORTF, 4}, // F4 21 + {&DDRF, &PINF, &PORTF, 1}, // F1 22 + {&DDRF, &PINF, &PORTF, 0}, // F0 23 + {&DDRD, &PIND, &PORTD, 4}, // D4 24 + {&DDRD, &PIND, &PORTD, 7}, // D7 25 + {&DDRB, &PINB, &PORTB, 4}, // B4 26 + {&DDRB, &PINB, &PORTB, 5}, // B5 27 + {&DDRB, &PINB, &PORTB, 6}, // B6 28 + {&DDRD, &PIND, &PORTD, 6} // D6 29 + }; + + #endif // !CORE_TEENSY + + #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) + + // Teensy++ 1.0 & 2.0 + static const pin_map_t pinMap[] = { + {&DDRD, &PIND, &PORTD, 0}, // D0 0 + {&DDRD, &PIND, &PORTD, 1}, // D1 1 + {&DDRD, &PIND, &PORTD, 2}, // D2 2 + {&DDRD, &PIND, &PORTD, 3}, // D3 3 + {&DDRD, &PIND, &PORTD, 4}, // D4 4 + {&DDRD, &PIND, &PORTD, 5}, // D5 5 + {&DDRD, &PIND, &PORTD, 6}, // D6 6 + {&DDRD, &PIND, &PORTD, 7}, // D7 7 + {&DDRE, &PINE, &PORTE, 0}, // E0 8 + {&DDRE, &PINE, &PORTE, 1}, // E1 9 + {&DDRC, &PINC, &PORTC, 0}, // C0 10 + {&DDRC, &PINC, &PORTC, 1}, // C1 11 + {&DDRC, &PINC, &PORTC, 2}, // C2 12 + {&DDRC, &PINC, &PORTC, 3}, // C3 13 + {&DDRC, &PINC, &PORTC, 4}, // C4 14 + {&DDRC, &PINC, &PORTC, 5}, // C5 15 + {&DDRC, &PINC, &PORTC, 6}, // C6 16 + {&DDRC, &PINC, &PORTC, 7}, // C7 17 + {&DDRE, &PINE, &PORTE, 6}, // E6 18 + {&DDRE, &PINE, &PORTE, 7}, // E7 19 + {&DDRB, &PINB, &PORTB, 0}, // B0 20 + {&DDRB, &PINB, &PORTB, 1}, // B1 21 + {&DDRB, &PINB, &PORTB, 2}, // B2 22 + {&DDRB, &PINB, &PORTB, 3}, // B3 23 + {&DDRB, &PINB, &PORTB, 4}, // B4 24 + {&DDRB, &PINB, &PORTB, 5}, // B5 25 + {&DDRB, &PINB, &PORTB, 6}, // B6 26 + {&DDRB, &PINB, &PORTB, 7}, // B7 27 + {&DDRA, &PINA, &PORTA, 0}, // A0 28 + {&DDRA, &PINA, &PORTA, 1}, // A1 29 + {&DDRA, &PINA, &PORTA, 2}, // A2 30 + {&DDRA, &PINA, &PORTA, 3}, // A3 31 + {&DDRA, &PINA, &PORTA, 4}, // A4 32 + {&DDRA, &PINA, &PORTA, 5}, // A5 33 + {&DDRA, &PINA, &PORTA, 6}, // A6 34 + {&DDRA, &PINA, &PORTA, 7}, // A7 35 + {&DDRE, &PINE, &PORTE, 4}, // E4 36 + {&DDRE, &PINE, &PORTE, 5}, // E5 37 + {&DDRF, &PINF, &PORTF, 0}, // F0 38 + {&DDRF, &PINF, &PORTF, 1}, // F1 39 + {&DDRF, &PINF, &PORTF, 2}, // F2 40 + {&DDRF, &PINF, &PORTF, 3}, // F3 41 + {&DDRF, &PINF, &PORTF, 4}, // F4 42 + {&DDRF, &PINF, &PORTF, 5}, // F5 43 + {&DDRF, &PINF, &PORTF, 6}, // F6 44 + {&DDRF, &PINF, &PORTF, 7} // F7 45 + }; + + #else // CPU type + + #error "Unknown CPU type for Software SPI" + + #endif // CPU type + + /** count of pins */ + static constexpr uint8_t digitalPinCount = sizeof(pinMap) / sizeof(pin_map_t); + + /** generate bad pin number error */ + void badPinNumber() + __attribute__((error("Pin number is too large or not a constant"))); + + /** + * Check for valid pin number + * @param[in] pin Number of pin to be checked. + */ + FORCE_INLINE static void badPinCheck(const uint8_t pin) { + if (!__builtin_constant_p(pin) || pin >= digitalPinCount) badPinNumber(); + } + + /** + * Fast write helper + * @param[in] address I/O register address + * @param[in] bit bit number to write + * @param[in] level value for bit + */ + FORCE_INLINE static void fastBitWriteSafe(volatile uint8_t* address, uint8_t bit, bool level) { + uint8_t oldSREG; + if (address > (uint8_t*)0x5F) { oldSREG = SREG; cli(); } + if (level) SBI(*address, bit); else CBI(*address, bit); + if (address > (uint8_t*)0x5F) SREG = oldSREG; + } + + /** + * Read pin value + * @param[in] pin Arduino pin number + * @return value read + */ + FORCE_INLINE static bool fastDigitalRead(uint8_t pin) { + badPinCheck(pin); + return (*pinMap[pin].pin >> pinMap[pin].bit) & 1; + } + + /** + * Toggle a pin + * @param[in] pin Arduino pin number + * + * If the pin is in output mode toggle the pin level. + * If the pin is in input mode toggle the state of the 20K pullup. + */ + FORCE_INLINE static void fastDigitalToggle(uint8_t pin) { + badPinCheck(pin); + if (pinMap[pin].pin > (uint8_t*)0x5F) + *pinMap[pin].pin = _BV(pinMap[pin].bit); // Must write bit to high address port + else + SBI(*pinMap[pin].pin, pinMap[pin].bit); // Compiles to sbi and PIN register will not be read + } + + /** + * Set pin value + * @param[in] pin Arduino pin number + * @param[in] level value to write + */ + FORCE_INLINE static void fastDigitalWrite(uint8_t pin, bool level) { + badPinCheck(pin); + fastBitWriteSafe(pinMap[pin].port, pinMap[pin].bit, level); + } + + /** + * Set pin mode + * @param[in] pin Arduino pin number + * @param[in] mode if true set output mode else input mode + * + * fastPinMode does not enable or disable the 20K pullup for input mode. + */ + FORCE_INLINE static void fastPinMode(uint8_t pin, bool mode) { + badPinCheck(pin); + fastBitWriteSafe(pinMap[pin].ddr, pinMap[pin].bit, mode); + } + +#endif // !__arm__ + +/** + * Set pin configuration + * @param[in] pin Arduino pin number + * @param[in] mode If true set output mode else input mode + * @param[in] level If mode is output, set level high/low. + * If mode is input, enable or disable the pin's 20K pullup. + */ +FORCE_INLINE static void fastPinConfig(uint8_t pin, bool mode, bool level) { + fastPinMode(pin, mode); + fastDigitalWrite(pin, level); +} + +/** + * @class DigitalPin + * @brief Fast digital port I/O + */ +template<uint8_t PinNumber> +class DigitalPin { +public: + + /** Constructor */ + DigitalPin() {} + + /** + * Constructor + * @param[in] pinMode if true set output mode else input mode. + */ + explicit DigitalPin(bool pinMode) { mode(pinMode); } + + /** + * Constructor + * @param[in] mode If true set output mode else input mode + * @param[in] level If mode is output, set level high/low. + * If mode is input, enable or disable the pin's 20K pullup. + */ + DigitalPin(bool mode, bool level) { config(mode, level); } + + /** + * Assignment operator + * @param[in] value If true set the pin's level high else set the + * pin's level low. + * + * @return This DigitalPin instance. + */ + FORCE_INLINE DigitalPin & operator = (bool value) { write(value); return *this; } + + /** + * Parentheses operator + * @return Pin's level + */ + FORCE_INLINE operator bool () const { return read(); } + + /** + * Set pin configuration + * @param[in] mode If true set output mode else input mode + * @param[in] level If mode is output, set level high/low. + * If mode is input, enable or disable the pin's 20K pullup. + */ + FORCE_INLINE void config(bool mode, bool level) { fastPinConfig(PinNumber, mode, level); } + + /** + * Set pin level high if output mode or enable 20K pullup if input mode. + */ + FORCE_INLINE void high() { write(true); } + + /** + * Set pin level low if output mode or disable 20K pullup if input mode. + */ + FORCE_INLINE void low() { write(false); } + + /** + * Set pin mode + * @param[in] pinMode if true set output mode else input mode. + * + * mode() does not enable or disable the 20K pullup for input mode. + */ + FORCE_INLINE void mode(bool pinMode) { fastPinMode(PinNumber, pinMode); } + + /** @return Pin's level */ + FORCE_INLINE bool read() const { return fastDigitalRead(PinNumber); } + + /** + * Toggle a pin + * If the pin is in output mode toggle the pin's level. + * If the pin is in input mode toggle the state of the 20K pullup. + */ + FORCE_INLINE void toggle() { fastDigitalToggle(PinNumber); } + + /** + * Write the pin's level. + * @param[in] value If true set the pin's level high else set the + * pin's level low. + */ + FORCE_INLINE void write(bool value) { fastDigitalWrite(PinNumber, value); } +}; + +const bool MISO_MODE = false, // Pin Mode for MISO is input. + MISO_LEVEL = false, // Pullups disabled for MISO are disabled. + MOSI_MODE = true, // Pin Mode for MOSI is output. + SCK_MODE = true; // Pin Mode for SCK is output. + +/** + * @class SoftSPI + * @brief Fast software SPI. + */ +template<uint8_t MisoPin, uint8_t MosiPin, uint8_t SckPin, uint8_t Mode = 0> +class SoftSPI { + public: + + /** Initialize SoftSPI pins. */ + void begin() { + fastPinConfig(MisoPin, MISO_MODE, MISO_LEVEL); + fastPinConfig(MosiPin, MOSI_MODE, !MODE_CPHA(Mode)); + fastPinConfig(SckPin, SCK_MODE, MODE_CPOL(Mode)); + } + + /** + * Soft SPI receive byte. + * @return Data byte received. + */ + FORCE_INLINE uint8_t receive() { + uint8_t data = 0; + receiveBit(7, &data); + receiveBit(6, &data); + receiveBit(5, &data); + receiveBit(4, &data); + receiveBit(3, &data); + receiveBit(2, &data); + receiveBit(1, &data); + receiveBit(0, &data); + return data; + } + + /** + * Soft SPI send byte. + * @param[in] data Data byte to send. + */ + FORCE_INLINE void send(uint8_t data) { + sendBit(7, data); + sendBit(6, data); + sendBit(5, data); + sendBit(4, data); + sendBit(3, data); + sendBit(2, data); + sendBit(1, data); + sendBit(0, data); + } + + /** + * Soft SPI transfer byte. + * @param[in] txData Data byte to send. + * @return Data byte received. + */ + FORCE_INLINE uint8_t transfer(uint8_t txData) { + uint8_t rxData = 0; + transferBit(7, &rxData, txData); + transferBit(6, &rxData, txData); + transferBit(5, &rxData, txData); + transferBit(4, &rxData, txData); + transferBit(3, &rxData, txData); + transferBit(2, &rxData, txData); + transferBit(1, &rxData, txData); + transferBit(0, &rxData, txData); + return rxData; + } + + private: + + FORCE_INLINE bool MODE_CPHA(uint8_t mode) { return bool(mode & 1); } + FORCE_INLINE bool MODE_CPOL(uint8_t mode) { return bool(mode & 2); } + FORCE_INLINE void receiveBit(uint8_t bit, uint8_t* data) { + if (MODE_CPHA(Mode)) fastDigitalWrite(SckPin, !MODE_CPOL(Mode)); + nop; + nop; + fastDigitalWrite(SckPin, + MODE_CPHA(Mode) ? MODE_CPOL(Mode) : !MODE_CPOL(Mode)); + if (fastDigitalRead(MisoPin)) SBI(*data, bit); + if (!MODE_CPHA(Mode)) fastDigitalWrite(SckPin, MODE_CPOL(Mode)); + } + + FORCE_INLINE void sendBit(uint8_t bit, uint8_t data) { + if (MODE_CPHA(Mode)) fastDigitalWrite(SckPin, !MODE_CPOL(Mode)); + fastDigitalWrite(MosiPin, data & _BV(bit)); + fastDigitalWrite(SckPin, MODE_CPHA(Mode) ? MODE_CPOL(Mode) : !MODE_CPOL(Mode)); + nop; + nop; + if (!MODE_CPHA(Mode)) fastDigitalWrite(SckPin, MODE_CPOL(Mode)); + } + + FORCE_INLINE void transferBit(uint8_t bit, uint8_t* rxData, uint8_t txData) { + if (MODE_CPHA(Mode)) fastDigitalWrite(SckPin, !MODE_CPOL(Mode)); + fastDigitalWrite(MosiPin, txData & _BV(bit)); + fastDigitalWrite(SckPin, + MODE_CPHA(Mode) ? MODE_CPOL(Mode) : !MODE_CPOL(Mode)); + if (fastDigitalRead(MisoPin)) SBI(*rxData, bit); + if (!MODE_CPHA(Mode)) fastDigitalWrite(SckPin, MODE_CPOL(Mode)); + } + +}; diff --git a/Marlin/src/libs/stopwatch.cpp b/Marlin/src/libs/stopwatch.cpp new file mode 100644 index 0000000..601efe5 --- /dev/null +++ b/Marlin/src/libs/stopwatch.cpp @@ -0,0 +1,106 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "stopwatch.h" + +#include "../inc/MarlinConfig.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../lcd/extui/ui_api.h" +#endif + +Stopwatch::State Stopwatch::state; +millis_t Stopwatch::accumulator; +millis_t Stopwatch::startTimestamp; +millis_t Stopwatch::stopTimestamp; + +bool Stopwatch::stop() { + Stopwatch::debug(PSTR("stop")); + + if (isRunning() || isPaused()) { + TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerStopped()); + state = STOPPED; + stopTimestamp = millis(); + return true; + } + else return false; +} + +bool Stopwatch::pause() { + Stopwatch::debug(PSTR("pause")); + + if (isRunning()) { + TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerPaused()); + state = PAUSED; + stopTimestamp = millis(); + return true; + } + else return false; +} + +bool Stopwatch::start() { + Stopwatch::debug(PSTR("start")); + + TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerStarted()); + + if (!isRunning()) { + if (isPaused()) accumulator = duration(); + else reset(); + + state = RUNNING; + startTimestamp = millis(); + return true; + } + else return false; +} + +void Stopwatch::resume(const millis_t with_time) { + Stopwatch::debug(PSTR("resume")); + + reset(); + if ((accumulator = with_time)) state = RUNNING; +} + +void Stopwatch::reset() { + Stopwatch::debug(PSTR("reset")); + + state = STOPPED; + startTimestamp = 0; + stopTimestamp = 0; + accumulator = 0; +} + +millis_t Stopwatch::duration() { + return accumulator + MS_TO_SEC((isRunning() ? millis() : stopTimestamp) - startTimestamp); +} + +#if ENABLED(DEBUG_STOPWATCH) + + void Stopwatch::debug(const char func[]) { + if (DEBUGGING(INFO)) { + SERIAL_ECHOPGM("Stopwatch::"); + serialprintPGM(func); + SERIAL_ECHOLNPGM("()"); + } + } + +#endif diff --git a/Marlin/src/libs/stopwatch.h b/Marlin/src/libs/stopwatch.h new file mode 100644 index 0000000..b64a36a --- /dev/null +++ b/Marlin/src/libs/stopwatch.h @@ -0,0 +1,123 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +// Print debug messages with M111 S2 (Uses 156 bytes of PROGMEM) +//#define DEBUG_STOPWATCH + +#include "../core/macros.h" // for FORCE_INLINE + +#include <stdint.h> +typedef uint32_t millis_t; + +/** + * @brief Stopwatch class + * @details This class acts as a timer proving stopwatch functionality including + * the ability to pause the running time counter. + */ +class Stopwatch { + private: + enum State : char { STOPPED, RUNNING, PAUSED }; + + static Stopwatch::State state; + static millis_t accumulator; + static millis_t startTimestamp; + static millis_t stopTimestamp; + + public: + /** + * @brief Initialize the stopwatch + */ + FORCE_INLINE static void init() { reset(); } + + /** + * @brief Stop the stopwatch + * @details Stop the running timer, it will silently ignore the request if + * no timer is currently running. + * @return true on success + */ + static bool stop(); + static inline bool abort() { return stop(); } // Alias by default + + /** + * @brief Pause the stopwatch + * @details Pause the running timer, it will silently ignore the request if + * no timer is currently running. + * @return true on success + */ + static bool pause(); + + /** + * @brief Start the stopwatch + * @details Start the timer, it will silently ignore the request if the + * timer is already running. + * @return true on success + */ + static bool start(); + + /** + * @brief Resume the stopwatch + * @details Resume a timer from a given duration + */ + static void resume(const millis_t with_time); + + /** + * @brief Reset the stopwatch + * @details Reset all settings to their default values. + */ + static void reset(); + + /** + * @brief Check if the timer is running + * @details Return true if the timer is currently running, false otherwise. + * @return true if stopwatch is running + */ + FORCE_INLINE static bool isRunning() { return state == RUNNING; } + + /** + * @brief Check if the timer is paused + * @details Return true if the timer is currently paused, false otherwise. + * @return true if stopwatch is paused + */ + FORCE_INLINE static bool isPaused() { return state == PAUSED; } + + /** + * @brief Get the running time + * @details Return the total number of seconds the timer has been running. + * @return the delta since starting the stopwatch + */ + static millis_t duration(); + + #ifdef DEBUG_STOPWATCH + + /** + * @brief Print a debug message + * @details Print a simple debug message "Stopwatch::function" + */ + static void debug(const char func[]); + + #else + + static inline void debug(const char[]) {} + + #endif +}; diff --git a/Marlin/src/libs/vector_3.cpp b/Marlin/src/libs/vector_3.cpp new file mode 100644 index 0000000..6f87a52 --- /dev/null +++ b/Marlin/src/libs/vector_3.cpp @@ -0,0 +1,156 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +/** + * vector_3.cpp - Vector library for bed leveling + * Copyright (c) 2012 Lars Brubaker. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "../inc/MarlinConfig.h" + +#if ABL_PLANAR || ENABLED(AUTO_BED_LEVELING_UBL) + +#include "vector_3.h" + +#include <math.h> + +/** + * vector_3 + */ + +vector_3 vector_3::cross(const vector_3 &left, const vector_3 &right) { + const xyz_float_t &lv = left, &rv = right; + return vector_3(lv.y * rv.z - lv.z * rv.y, // YZ cross + lv.z * rv.x - lv.x * rv.z, // ZX cross + lv.x * rv.y - lv.y * rv.x); // XY cross +} + +vector_3 vector_3::get_normal() const { + vector_3 normalized = *this; + normalized.normalize(); + return normalized; +} + +void vector_3::normalize() { + *this *= RSQRT(sq(x) + sq(y) + sq(z)); +} + +// Apply a rotation to the matrix +void vector_3::apply_rotation(const matrix_3x3 &matrix) { + const float _x = x, _y = y, _z = z; + *this = { matrix.vectors[0][0] * _x + matrix.vectors[1][0] * _y + matrix.vectors[2][0] * _z, + matrix.vectors[0][1] * _x + matrix.vectors[1][1] * _y + matrix.vectors[2][1] * _z, + matrix.vectors[0][2] * _x + matrix.vectors[1][2] * _y + matrix.vectors[2][2] * _z }; +} + +void vector_3::debug(PGM_P const title) { + serialprintPGM(title); + SERIAL_ECHOPAIR_F_P(SP_X_STR, x, 6); + SERIAL_ECHOPAIR_F_P(SP_Y_STR, y, 6); + SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z, 6); +} + +/** + * matrix_3x3 + */ + +void apply_rotation_xyz(const matrix_3x3 &matrix, float &_x, float &_y, float &_z) { + vector_3 vec = vector_3(_x, _y, _z); vec.apply_rotation(matrix); + _x = vec.x; _y = vec.y; _z = vec.z; +} + +// Reset to identity. No rotate or translate. +void matrix_3x3::set_to_identity() { + LOOP_L_N(i, 3) + LOOP_L_N(j, 3) + vectors[i][j] = float(i == j); +} + +// Create a matrix from 3 vector_3 inputs +matrix_3x3 matrix_3x3::create_from_rows(const vector_3 &row_0, const vector_3 &row_1, const vector_3 &row_2) { + //row_0.debug(PSTR("row_0")); + //row_1.debug(PSTR("row_1")); + //row_2.debug(PSTR("row_2")); + matrix_3x3 new_matrix; + new_matrix.vectors[0] = row_0; + new_matrix.vectors[1] = row_1; + new_matrix.vectors[2] = row_2; + //new_matrix.debug(PSTR("new_matrix")); + return new_matrix; +} + +// Create a matrix rotated to point towards a target +matrix_3x3 matrix_3x3::create_look_at(const vector_3 &target) { + const vector_3 z_row = target.get_normal(), + x_row = vector_3(1, 0, -target.x / target.z).get_normal(), + y_row = vector_3::cross(z_row, x_row).get_normal(); + + // x_row.debug(PSTR("x_row")); + // y_row.debug(PSTR("y_row")); + // z_row.debug(PSTR("z_row")); + + // create the matrix already correctly transposed + matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); + + // rot.debug(PSTR("rot")); + return rot; +} + +// Get a transposed copy of the matrix +matrix_3x3 matrix_3x3::transpose(const matrix_3x3 &original) { + matrix_3x3 new_matrix; + LOOP_L_N(i, 3) + LOOP_L_N(j, 3) + new_matrix.vectors[i][j] = original.vectors[j][i]; + return new_matrix; +} + +void matrix_3x3::debug(PGM_P const title) { + if (title) { + serialprintPGM(title); + SERIAL_EOL(); + } + LOOP_L_N(i, 3) { + LOOP_L_N(j, 3) { + if (vectors[i][j] >= 0.0) SERIAL_CHAR('+'); + SERIAL_ECHO_F(vectors[i][j], 6); + SERIAL_CHAR(' '); + } + SERIAL_EOL(); + } +} + +#endif // HAS_ABL_OR_UBL diff --git a/Marlin/src/libs/vector_3.h b/Marlin/src/libs/vector_3.h new file mode 100644 index 0000000..5842831 --- /dev/null +++ b/Marlin/src/libs/vector_3.h @@ -0,0 +1,90 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +/** + * vector_3.cpp - Vector library for bed leveling + * Copyright (c) 2012 Lars Brubaker. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "../core/types.h" + +class matrix_3x3; + +struct vector_3 : xyz_float_t { + + vector_3(const float &_x, const float &_y, const float &_z) { set(_x, _y, _z); } + vector_3(const xy_float_t &in) { set(in.x, in.y); } + vector_3(const xyz_float_t &in) { set(in.x, in.y, in.z); } + vector_3(const xyze_float_t &in) { set(in.x, in.y, in.z); } + vector_3() { reset(); } + + // Factory method + static vector_3 cross(const vector_3 &a, const vector_3 &b); + + // Modifiers + void normalize(); + void apply_rotation(const matrix_3x3 &matrix); + + // Accessors + float get_length() const; + vector_3 get_normal() const; + + // Operators + FORCE_INLINE vector_3 operator+(const vector_3 &v) const { vector_3 o = *this; o += v; return o; } + FORCE_INLINE vector_3 operator-(const vector_3 &v) const { vector_3 o = *this; o -= v; return o; } + FORCE_INLINE vector_3 operator*(const float &v) const { vector_3 o = *this; o *= v; return o; } + + void debug(PGM_P const title); +}; + +struct matrix_3x3 { + abc_float_t vectors[3]; + + // Factory methods + static matrix_3x3 create_from_rows(const vector_3 &row_0, const vector_3 &row_1, const vector_3 &row_2); + static matrix_3x3 create_look_at(const vector_3 &target); + static matrix_3x3 transpose(const matrix_3x3 &original); + + void set_to_identity(); + + void debug(PGM_P const title); +}; + +void apply_rotation_xyz(const matrix_3x3 &rotationMatrix, float &x, float &y, float &z); +FORCE_INLINE void apply_rotation_xyz(const matrix_3x3 &rotationMatrix, xyz_pos_t &pos) { + apply_rotation_xyz(rotationMatrix, pos.x, pos.y, pos.z); +} |