Skip to content

Instantly share code, notes, and snippets.

@Enverex
Created May 28, 2019 13:39
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save Enverex/5e5ea90616c991e0b6a177536f3f1067 to your computer and use it in GitHub Desktop.
Config, Makefile and "date change only" files stripped out
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL.cpp
index 1534a35..7a27f2a 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL.cpp
@@ -1,7 +1,7 @@
/**
* Marlin 3D Printer Firmware
*
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
* Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com
* Copyright (c) 2017 Victor Perez
@@ -34,6 +34,9 @@
#include "HAL.h"
#include <STM32ADC.h>
#include "../../inc/MarlinConfig.h"
+#include <SPI.h>
+//#include <Wire.h>
+//#include <libmaple/I2C.h>
// --------------------------------------------------------------------------
// Externals
@@ -92,9 +95,7 @@
// --------------------------------------------------------------------------
// Public Variables
// --------------------------------------------------------------------------
-#ifdef SERIAL_USB
- USBSerial SerialUSB;
-#endif
+USBSerial SerialUSB;
uint16_t HAL_adc_result;
@@ -103,16 +104,10 @@ uint16_t HAL_adc_result;
// --------------------------------------------------------------------------
STM32ADC adc(ADC1);
-uint8_t adc_pins[] = {
+uint8 adc_pins[] = {
#if HAS_TEMP_ADC_0
TEMP_0_PIN,
#endif
- #if HAS_HEATED_BED
- TEMP_BED_PIN,
- #endif
- #if HAS_HEATED_CHAMBER
- TEMP_CHAMBER_PIN,
- #endif
#if HAS_TEMP_ADC_1
TEMP_1_PIN,
#endif
@@ -125,8 +120,8 @@ uint8_t adc_pins[] = {
#if HAS_TEMP_ADC_4
TEMP_4_PIN,
#endif
- #if HAS_TEMP_ADC_5
- TEMP_5_PIN,
+ #if HAS_HEATED_BED
+ TEMP_BED_PIN,
#endif
#if ENABLED(FILAMENT_WIDTH_SENSOR)
FILWIDTH_PIN,
@@ -137,12 +132,6 @@ enum TEMP_PINS : char {
#if HAS_TEMP_ADC_0
TEMP_0,
#endif
- #if HAS_HEATED_BED
- TEMP_BED,
- #endif
- #if HAS_HEATED_CHAMBER
- TEMP_CHAMBER,
- #endif
#if HAS_TEMP_ADC_1
TEMP_1,
#endif
@@ -155,8 +144,8 @@ enum TEMP_PINS : char {
#if HAS_TEMP_ADC_4
TEMP_4,
#endif
- #if HAS_TEMP_ADC_5
- TEMP_5,
+ #if HAS_HEATED_BED
+ TEMP_BED,
#endif
#if ENABLED(FILAMENT_WIDTH_SENSOR)
FILWIDTH,
@@ -207,8 +196,49 @@ static void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) {
} }
#endif
+void PWM_fan_init(void) {
+ HardwareTimer pwmtimer(8);
+ pwmtimer.pause();
+ pwmtimer.setPrescaleFactor(1);
+ pwmtimer.setOverflow(0xFFFF);
+ pwmtimer.refresh();
+ pwmtimer.resume();
+}
+
+void PWM_vref_init(void) {
+ HardwareTimer pwmtimer(3);
+ pwmtimer.pause();
+ pwmtimer.setPrescaleFactor(1);
+ pwmtimer.setOverflow(2000);
+ pwmtimer.refresh();
+ pwmtimer.resume();
+}
+
void HAL_init(void) {
+
NVIC_SetPriorityGrouping(0x3);
+
+#if SPI_MODULE
+ SPI.setModule(SPI_MODULE);
+#endif
+
+#if MB(MKS_ROBIN_MINI)
+ //PWM_fan_init();
+// #if PIN_EXISTS(VREF_XY)
+// SET_PWM(VREF_XY_PIN);
+// pwmWrite(VREF_XY_PIN, VREF_XY_VALUE);
+// #endif
+// #if PIN_EXISTS(VREF_Z)
+// SET_PWM(VREF_Z_PIN);
+// pwmWrite(VREF_Z_PIN, VREF_Z_VALUE);
+// #endif
+// #if PIN_EXISTS(VREF_E1)
+// SET_PWM(VREF_E1_PIN);
+// pwmWrite(VREF_E1_PIN, VREF_E1_VALUE);
+// #endif
+#endif
+//#if ENABLED(I2C_EEPROM)
+//#endif
}
/* VGPV Done with defines
@@ -282,12 +312,6 @@ void HAL_adc_start_conversion(const uint8_t adc_pin) {
#if HAS_TEMP_ADC_0
case TEMP_0_PIN: pin_index = TEMP_0; break;
#endif
- #if HAS_HEATED_BED
- case TEMP_BED_PIN: pin_index = TEMP_BED; break;
- #endif
- #if HAS_HEATED_CHAMBER
- case TEMP_CHAMBER_PIN: pin_index = TEMP_CHAMBER; break;
- #endif
#if HAS_TEMP_ADC_1
case TEMP_1_PIN: pin_index = TEMP_1; break;
#endif
@@ -300,8 +324,8 @@ void HAL_adc_start_conversion(const uint8_t adc_pin) {
#if HAS_TEMP_ADC_4
case TEMP_4_PIN: pin_index = TEMP_4; break;
#endif
- #if HAS_TEMP_ADC_5
- case TEMP_5_PIN: pin_index = TEMP_5; break;
+ #if HAS_HEATED_BED
+ case TEMP_BED_PIN: pin_index = TEMP_BED; break;
#endif
#if ENABLED(FILAMENT_WIDTH_SENSOR)
case FILWIDTH_PIN: pin_index = FILWIDTH; break;
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL.h
index 66c7ee8..41722dc 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL.h
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL.h
@@ -1,7 +1,7 @@
/**
* Marlin 3D Printer Firmware
*
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
* Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com
* Copyright (c) 2017 Victor Perez
@@ -36,6 +36,7 @@
// Includes
// --------------------------------------------------------------------------
+#include "../../core/macros.h"
#include "../shared/Marduino.h"
#include "../shared/math_32bit.h"
#include "../shared/HAL_SPI.h"
@@ -50,67 +51,45 @@
#include "../../inc/MarlinConfigPre.h"
+
// --------------------------------------------------------------------------
// Defines
// --------------------------------------------------------------------------
-#ifdef SERIAL_USB
- #define UsbSerial Serial
- #define MSerial1 Serial1
- #define MSerial2 Serial2
- #define MSerial3 Serial3
- #define MSerial4 Serial4
- #define MSerial5 Serial5
-#else
- extern USBSerial SerialUSB;
- #define UsbSerial SerialUSB
- #define MSerial1 Serial
- #define MSerial2 Serial1
- #define MSerial3 Serial2
- #define MSerial4 Serial3
- #define MSerial5 Serial4
-#endif
-
-#if !WITHIN(SERIAL_PORT, -1, 5)
- #error "SERIAL_PORT must be from -1 to 5"
+#if !WITHIN(SERIAL_PORT, -1, 3)
+ #error "SERIAL_PORT must be from -1 to 3"
#endif
#if SERIAL_PORT == -1
- #define MYSERIAL0 UsbSerial
+ extern USBSerial SerialUSB;
+ #define MYSERIAL0 SerialUSB
#elif SERIAL_PORT == 0
- #error "Serial port 0 does not exist"
+ #define MYSERIAL0 Serial
#elif SERIAL_PORT == 1
- #define MYSERIAL0 MSerial1
+ #define MYSERIAL0 Serial1
#elif SERIAL_PORT == 2
- #define MYSERIAL0 MSerial2
+ #define MYSERIAL0 Serial2
#elif SERIAL_PORT == 3
- #define MYSERIAL0 MSerial3
-#elif SERIAL_PORT == 4
- #define MYSERIAL0 MSerial4
-#elif SERIAL_PORT == 5
- #define MYSERIAL0 MSerial5
+ #define MYSERIAL0 Serial3
#endif
#ifdef SERIAL_PORT_2
- #if !WITHIN(SERIAL_PORT_2, -1, 5)
- #error "SERIAL_PORT_2 must be from -1 to 5"
+ #if !WITHIN(SERIAL_PORT_2, -1, 3)
+ #error "SERIAL_PORT_2 must be from -1 to 3"
#elif SERIAL_PORT_2 == SERIAL_PORT
#error "SERIAL_PORT_2 must be different than SERIAL_PORT"
#endif
#define NUM_SERIAL 2
#if SERIAL_PORT_2 == -1
- #define MYSERIAL1 UsbSerial
+ extern USBSerial SerialUSB;
+ #define MYSERIAL1 SerialUSB
#elif SERIAL_PORT_2 == 0
- #error "Serial port 0 does not exist"
+ #define MYSERIAL1 Serial
#elif SERIAL_PORT_2 == 1
- #define MYSERIAL1 MSerial1
+ #define MYSERIAL1 Serial1
#elif SERIAL_PORT_2 == 2
- #define MYSERIAL1 MSerial2
+ #define MYSERIAL1 Serial2
#elif SERIAL_PORT_2 == 3
- #define MYSERIAL1 MSerial3
- #elif SERIAL_PORT_2 == 4
- #define MYSERIAL1 MSerial4
- #elif SERIAL_PORT_2 == 5
- #define MYSERIAL1 MSerial5
+ #define MYSERIAL1 Serial3
#endif
#else
#define NUM_SERIAL 1
@@ -131,6 +110,7 @@ void HAL_init();
#define digitalPinHasPWM(P) (PIN_MAP[P].timer_device != NULL)
#endif
+
#define CRITICAL_SECTION_START uint32_t primask = __get_primask(); (void)__iCliRetVal()
#define CRITICAL_SECTION_END if (!primask) (void)__iSeiRetVal()
#define ISRS_ENABLED() (!__get_primask())
@@ -183,10 +163,10 @@ extern uint16_t HAL_adc_result;
#define __bss_end __bss_end__
/** clear reset reason */
-void HAL_clear_reset_source(void);
+void HAL_clear_reset_source (void);
/** reset reason */
-uint8_t HAL_get_reset_source(void);
+uint8_t HAL_get_reset_source (void);
void _delay_ms(const int delay);
@@ -215,10 +195,7 @@ static int freeMemory() {
#pragma GCC diagnostic pop
-//
// SPI: Extended functions which take a channel number (hardware SPI only)
-//
-
/** Write single byte to specified SPI channel */
void spiSend(uint32_t chan, byte b);
/** Write buffer to specified SPI channel */
@@ -226,22 +203,19 @@ void spiSend(uint32_t chan, const uint8_t* buf, size_t n);
/** Read single byte from specified SPI channel */
uint8_t spiRec(uint32_t chan);
-//
+
// EEPROM
-//
/**
- * TODO: Write all this EEPROM stuff. Can emulate EEPROM in flash as last resort.
- * Wire library should work for i2c EEPROMs.
+ * TODO: Write all this eeprom stuff. Can emulate eeprom in flash as last resort.
+ * Wire library should work for i2c eeproms.
*/
void eeprom_write_byte(uint8_t *pos, unsigned char value);
uint8_t eeprom_read_byte(uint8_t *pos);
-void eeprom_read_block(void *__dst, const void *__src, size_t __n);
-void eeprom_update_block(const void *__src, void *__dst, size_t __n);
+void eeprom_read_block (void *__dst, const void *__src, size_t __n);
+void eeprom_update_block (const void *__src, void *__dst, size_t __n);
-//
// ADC
-//
#define HAL_ANALOG_SELECT(pin) pinMode(pin, INPUT_ANALOG);
@@ -275,3 +249,6 @@ void HAL_enable_AdcFreerun(void);
#define JTAG_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_SW_ONLY)
#define JTAGSWD_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_NONE)
+
+void PWM_vref_init(void);
+void PWM_fan_init(void);
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp b/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp
deleted file mode 100644
index f8fc315..0000000
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- * Copyright (C) 2017 Victor Perez
- *
- * 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 <http://www.gnu.org/licenses/>.
- *
- */
-
-#ifdef __STM32F1__
-
-#include "../../inc/MarlinConfig.h"
-
-#if HAS_SERVOS
-
-uint8_t ServoCount; //=0
-
-#include "HAL_Servo_STM32F1.h"
-
-//#include "Servo.h"
-
-#include <boards.h>
-#include <io.h>
-#include <pwm.h>
-#include <wirish_math.h>
-
-/**
- * 20 millisecond period config. For a 1-based prescaler,
- *
- * (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec
- * => prescaler * overflow = 20 * CYC_MSEC
- *
- * This uses the smallest prescaler that allows an overflow < 2^16.
- */
-#define MAX_OVERFLOW ((1 << 16) - 1)
-#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND)
-#define TAU_MSEC 20
-#define TAU_USEC (TAU_MSEC * 1000)
-#define TAU_CYC (TAU_MSEC * CYC_MSEC)
-#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1)
-#define SERVO_OVERFLOW ((uint16_t)round((double)TAU_CYC / SERVO_PRESCALER))
-
-// Unit conversions
-#define US_TO_COMPARE(us) ((uint16_t)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW))
-#define COMPARE_TO_US(c) ((uint32_t)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC))
-#define ANGLE_TO_US(a) ((uint16_t)(map((a), this->minAngle, this->maxAngle, \
- SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW)))
-#define US_TO_ANGLE(us) ((int16_t)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, \
- this->minAngle, this->maxAngle)))
-
-libServo::libServo() {
- this->servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO;
-}
-
-bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) {
- if (this->servoIndex >= MAX_SERVOS) return false;
-
- this->pin = pin;
- this->minAngle = minAngle;
- this->maxAngle = maxAngle;
-
- timer_dev *tdev = PIN_MAP[this->pin].timer_device;
- uint8_t tchan = PIN_MAP[this->pin].timer_channel;
-
- pinMode(this->pin, PWM);
- pwmWrite(this->pin, 0);
-
- timer_pause(tdev);
- timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
- timer_set_reload(tdev, SERVO_OVERFLOW);
- timer_generate_update(tdev);
- timer_resume(tdev);
-
- return true;
-}
-
-bool libServo::detach() {
- if (!this->attached()) return false;
- pwmWrite(this->pin, 0);
- return true;
-}
-
-int32_t libServo::read() const {
- if (this->attached()) {
- timer_dev *tdev = PIN_MAP[this->pin].timer_device;
- uint8_t tchan = PIN_MAP[this->pin].timer_channel;
- return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan)));
- }
- return 0;
-}
-
-void libServo::move(const int32_t value) {
- constexpr uint16_t servo_delay[] = SERVO_DELAY;
- static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
-
- if (this->attached()) {
- pwmWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(constrain(value, this->minAngle, this->maxAngle))));
- safe_delay(servo_delay[this->servoIndex]);
- #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
- this->detach();
- #endif
- }
-}
-#endif // HAS_SERVOS
-
-#endif // __STM32F1__
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h b/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h
deleted file mode 100644
index ac83f08..0000000
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- * Copyright (C) 2017 Victor Perez
- *
- * 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 <http://www.gnu.org/licenses/>.
- *
- */
-#pragma once
-
-// Pin number of unattached pins
-#define NOT_ATTACHED (-1)
-#define INVALID_SERVO 255
-
-#ifndef MAX_SERVOS
- #define MAX_SERVOS 3
-#endif
-
-#define SERVO_DEFAULT_MIN_PW 544
-#define SERVO_DEFAULT_MAX_PW 2400
-#define SERVO_DEFAULT_MIN_ANGLE 0
-#define SERVO_DEFAULT_MAX_ANGLE 180
-
-#define HAL_SERVO_LIB libServo
-
-class libServo {
- public:
- libServo();
- bool attach(const int32_t pin, const int32_t minAngle=SERVO_DEFAULT_MIN_ANGLE, const int32_t maxAngle=SERVO_DEFAULT_MAX_ANGLE);
- bool attached() const { return this->pin != NOT_ATTACHED; }
- bool detach();
- void move(const int32_t value);
- int32_t read() const;
- private:
- uint8_t servoIndex; // index into the channel data for this servo
- int32_t pin = NOT_ATTACHED;
- int32_t minAngle;
- int32_t maxAngle;
-};
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.cpp
index d7aa3cc..27c709a 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.cpp
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -21,7 +21,7 @@
*
*/
-#if defined(ARDUINO_ARCH_STM32F1) && (defined(STM32_HIGH_DENSITY) || defined(STM32_XL_DENSITY))
+#ifdef __STM32F1__
#include "HAL_sdio_STM32F1.h"
@@ -73,6 +73,7 @@ bool SDIO_Init(void) {
sdio_set_dbus_width(SDIO_CLKCR_WIDBUS_4BIT);
sdio_set_clock(SDIO_CLOCK);
+
return true;
}
@@ -116,7 +117,9 @@ bool SDIO_ReadBlock_DMA(uint32_t blockAddress, uint8_t *data) {
bool SDIO_ReadBlock(uint32_t blockAddress, uint8_t *data) {
uint32_t retries = 3;
- while (retries--) if (SDIO_ReadBlock_DMA(blockAddress, data)) return true;
+ while (retries--) {
+ if (SDIO_ReadBlock_DMA(blockAddress, data)) return true;
+ }
return false;
}
@@ -150,7 +153,7 @@ bool SDIO_WriteBlock(uint32_t blockAddress, const uint8_t *data) {
SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS);
- uint32_t timeout = millis() + SDIO_WRITE_TIMEOUT;
+ uint32 timeout = millis() + SDIO_WRITE_TIMEOUT;
while (timeout > millis()) {
if (SDIO_GetCardState() == SDIO_CARD_TRANSFER) {
return true;
@@ -276,4 +279,4 @@ bool SDIO_GetCmdResp7(void) {
return true;
}
-#endif // ARDUINO_ARCH_STM32F1 && (STM32_HIGH_DENSITY || STM32_XL_DENSITY)
+#endif // __STM32F1__
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.h
index bb48b3b..034bd6d 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.h
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_sdio_STM32F1.h
@@ -1,7 +1,7 @@
/**
* Marlin 3D Printer Firmware
*
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
* Copyright (c) 2017 Victor Perez
*
@@ -25,8 +25,7 @@
// Includes
// --------------------------------------------------------------------------
-#include "../shared/Marduino.h"
-
+#include "Arduino.h"
#include "libmaple/sdio.h"
#include "libmaple/dma.h"
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_spi_STM32F1.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_spi_STM32F1.cpp
index cbc6c7f..c2e09cf 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_spi_STM32F1.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_spi_STM32F1.cpp
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -86,7 +86,7 @@ void spiBegin() {
}
/**
- * @brief Initialize SPI port to required speed rate and transfer mode (MSB, SPI MODE 0)
+ * @brief Initializes SPI port to required speed rate and transfer mode (MSB, SPI MODE 0)
*
* @param spiRate Rate as declared in HAL.h (speed do not match AVR)
* @return Nothing
@@ -109,7 +109,7 @@ void spiInit(uint8_t spiRate) {
}
/**
- * @brief Receive a single byte from the SPI port.
+ * @brief Receives a single byte from the SPI port.
*
* @return Byte received
*
@@ -123,7 +123,7 @@ uint8_t spiRec(void) {
}
/**
- * @brief Receive a number of bytes from the SPI port to a buffer
+ * @brief Receives 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.
@@ -133,12 +133,12 @@ uint8_t spiRec(void) {
*/
void spiRead(uint8_t* buf, uint16_t nbyte) {
SPI.beginTransaction(spiConfig);
- SPI.dmaTransfer(0, const_cast<uint8_t*>(buf), nbyte);
+ SPI.dmaTransfer(0, const_cast<uint8*>(buf), nbyte);
SPI.endTransaction();
}
/**
- * @brief Send a single byte on SPI port
+ * @brief Sends a single byte on SPI port
*
* @param b Byte to send
*
@@ -161,20 +161,11 @@ void spiSend(uint8_t b) {
void spiSendBlock(uint8_t token, const uint8_t* buf) {
SPI.beginTransaction(spiConfig);
SPI.send(token);
- SPI.dmaSend(const_cast<uint8_t*>(buf), 512);
+ SPI.dmaSend(const_cast<uint8*>(buf), 512);
SPI.endTransaction();
}
-/**
- * @brief Begin SPI transaction, set clock, bit order, data mode
- *
- * @param spiClock Clock setting
- * @param bitOrder Bit Order setting
- * @param dataMode Data Mode setting
- * @return Nothing
- *
- * @details Uses an SPI Config via SPISettings
- */
+/** Begin SPI transaction, set clock, bit order, data mode */
void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) {
spiConfig = SPISettings(spiClock, (BitOrder)bitOrder, dataMode);
SPI.beginTransaction(spiConfig);
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_timers_STM32F1.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_timers_STM32F1.cpp
index 8b235c0..f88e46b 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/HAL_timers_STM32F1.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/HAL_timers_STM32F1.cpp
@@ -1,7 +1,7 @@
/**
* Marlin 3D Printer Firmware
*
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
* Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com
*
@@ -119,7 +119,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
case STEP_TIMER_NUM:
timer_pause(STEP_TIMER_DEV);
timer_set_count(STEP_TIMER_DEV, 0);
- timer_set_prescaler(STEP_TIMER_DEV, (uint16_t)(STEPPER_TIMER_PRESCALE - 1));
+ timer_set_prescaler(STEP_TIMER_DEV, (uint16)(STEPPER_TIMER_PRESCALE - 1));
timer_set_reload(STEP_TIMER_DEV, 0xFFFF);
timer_set_compare(STEP_TIMER_DEV, STEP_TIMER_CHAN, MIN(HAL_TIMER_TYPE_MAX, (STEPPER_TIMER_RATE / frequency)));
timer_attach_interrupt(STEP_TIMER_DEV, STEP_TIMER_CHAN, stepTC_Handler);
@@ -130,7 +130,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
case TEMP_TIMER_NUM:
timer_pause(TEMP_TIMER_DEV);
timer_set_count(TEMP_TIMER_DEV, 0);
- timer_set_prescaler(TEMP_TIMER_DEV, (uint16_t)(TEMP_TIMER_PRESCALE - 1));
+ timer_set_prescaler(TEMP_TIMER_DEV, (uint16)(TEMP_TIMER_PRESCALE - 1));
timer_set_reload(TEMP_TIMER_DEV, 0xFFFF);
timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, MIN(HAL_TIMER_TYPE_MAX, ((F_CPU / TEMP_TIMER_PRESCALE) / frequency)));
timer_attach_interrupt(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, tempTC_Handler);
@@ -157,7 +157,7 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num) {
}
}
-static inline bool timer_irq_enabled(const timer_dev * const dev, const uint8_t interrupt) {
+static inline bool timer_irq_enabled(const timer_dev * const dev, const uint8 interrupt) {
return bool(*bb_perip(&(dev->regs).adv->DIER, interrupt));
}
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/SanityCheck.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/SanityCheck.h
index c1470ca..ff38fa9 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/SanityCheck.h
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/SanityCheck.h
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -28,8 +28,8 @@
* Test Re-ARM specific configuration values for errors at compile-time.
*/
#if ENABLED(SPINDLE_LASER_ENABLE)
- #if !PIN_EXISTS(SPINDLE_LASER_ENA)
- #error "SPINDLE_LASER_ENABLE requires SPINDLE_LASER_ENA_PIN."
+ #if !PIN_EXISTS(SPINDLE_LASER_ENABLE)
+ #error "SPINDLE_LASER_ENABLE requires SPINDLE_LASER_ENABLE_PIN."
#elif SPINDLE_DIR_CHANGE && !PIN_EXISTS(SPINDLE_DIR)
#error "SPINDLE_DIR_PIN not defined."
#elif ENABLED(SPINDLE_LASER_PWM) && PIN_EXISTS(SPINDLE_LASER_PWM)
@@ -75,6 +75,6 @@
#error "SDIO_SUPPORT requires SDSUPPORT. Enable SDSUPPORT to continue."
#endif
-#if ENABLED(FAST_PWM_FAN)
- #error "FAST_PWM_FAN is not yet implemented for this platform."
+#if (PIN_EXISTS(VREF_XY) && !defined(VREF_XY_VALUE)) || (PIN_EXISTS(VREF_Z) && !defined(VREF_Z_VALUE)) || (PIN_EXISTS(VREF_E1) && !defined(VREF_E1_VALUE))
+ #error "VREF_XX is defined but PWM value not set."
#endif
diff --git a/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/binary.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/binary.h
new file mode 100644
index 0000000..70d5ead
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/binary.h
@@ -0,0 +1 @@
+#include "bit_constants.h"
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h
index 3994b32..3be336f 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -30,7 +30,7 @@
#include <libmaple/gpio.h>
#define READ(IO) (PIN_MAP[IO].gpio_device->regs->IDR & (1U << PIN_MAP[IO].gpio_bit) ? HIGH : LOW)
-#define WRITE(IO,V) (PIN_MAP[IO].gpio_device->regs->BSRR = (1U << PIN_MAP[IO].gpio_bit) << (16 * !((bool)V)))
+#define WRITE(IO,V) (PIN_MAP[IO].gpio_device->regs->BSRR = (1U << PIN_MAP[IO].gpio_bit) << (16 * !(bool)V))
#define TOGGLE(IO) (PIN_MAP[IO].gpio_device->regs->ODR = PIN_MAP[IO].gpio_device->regs->ODR ^ (1U << PIN_MAP[IO].gpio_bit))
#define WRITE_VAR(IO,V) WRITE(IO,V)
@@ -42,17 +42,16 @@
#define SET_INPUT(IO) _SET_MODE(IO, GPIO_INPUT_FLOATING)
#define SET_INPUT_PULLUP(IO) _SET_MODE(IO, GPIO_INPUT_PU)
-#define SET_OUTPUT(IO) OUT_WRITE(IO, LOW)
+#define SET_OUTPUT(IO) OUT_WRITE(IO,LOW)
#define SET_PWM(IO) pinMode(IO, PWM) // do{ gpio_set_mode(PIN_MAP[pin].gpio_device, PIN_MAP[pin].gpio_bit, GPIO_AF_OUTPUT_PP); timer_set_mode(PIN_MAP[pin].timer_device, PIN_MAP[pin].timer_channel, TIMER_PWM); }while(0)
-#define IS_INPUT(IO) (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD)
-#define IS_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP)
-#define HAS_TIMER(IO) (PIN_MAP[IO].timer_device != NULL)
+#define GET_INPUT(IO) (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD)
+#define GET_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP)
+#define GET_TIMER(IO) (PIN_MAP[IO].timer_device != NULL)
-#define PWM_PIN(P) HAS_TIMER(P)
-#define USEABLE_HARDWARE_PWM(P) PWM_PIN(P)
+#define PWM_PIN(p) true
+#define USEABLE_HARDWARE_PWM(p) PWM_PIN(p)
// digitalRead/Write wrappers
#define extDigitalRead(IO) digitalRead(IO)
#define extDigitalWrite(IO,V) digitalWrite(IO,V)
-
diff --git a/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_eeprom.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_eeprom.cpp
new file mode 100644
index 0000000..f39f295
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_eeprom.cpp
@@ -0,0 +1,46 @@
+#ifdef __STM32F1__
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(EEPROM_SETTINGS) && ENABLED(I2C_EEPROM)
+
+#include "../shared/persistent_store_api.h"
+
+bool PersistentStore::access_start() { return true; }
+bool PersistentStore::access_finish() { return true; }
+
+bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) {
+ while (size--) {
+ uint8_t * const p = (uint8_t * const)pos;
+ uint8_t v = *value;
+ // EEPROM has only ~100,000 write cycles,
+ // so only write bytes that have changed!
+ if (v != eeprom_read_byte(p)) {
+ eeprom_write_byte(p, v);
+ if (eeprom_read_byte(p) != v) {
+ SERIAL_ECHO_MSG(MSG_ERR_EEPROM_WRITE);
+ return true;
+ }
+ }
+ crc16(crc, &v, 1);
+ pos++;
+ value++;
+ };
+ return false;
+}
+
+bool PersistentStore::read_data(int &pos, uint8_t* value, size_t size, uint16_t *crc, const bool writing/*=true*/) {
+ do {
+ uint8_t c = eeprom_read_byte((uint8_t*)pos);
+ if (writing) *value = c;
+ crc16(crc, &c, 1);
+ pos++;
+ value++;
+ } while (--size);
+ return false; // always assume success for AVR's
+}
+
+size_t PersistentStore::capacity() { return E2END + 1; }
+
+#endif // EEPROM_SETTINGS || I2C_EEPROM
+#endif // __STM32F1__
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_flash.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_flash.cpp
index aa03474..43e1721 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_flash.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_flash.cpp
@@ -1,7 +1,7 @@
/**
* Marlin 3D Printer Firmware
*
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
* Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com
* Copyright (c) 2016 Victor Perez victor_pv@hotmail.com
@@ -31,74 +31,97 @@
#include "../../inc/MarlinConfig.h"
-// This is for EEPROM emulation in flash
#if BOTH(EEPROM_SETTINGS, FLASH_EEPROM_EMULATION)
-#include "../shared/persistent_store_api.h"
+#define EEPROM_ERASE ((uint16)0xFF)
+#include "../shared/persistent_store_api.h"
#include <flash_stm32.h>
#include <EEPROM.h>
+static uint8_t ram_eeprom[EEPROM_SIZE] __attribute__((aligned(4))) = {0};
+static bool eeprom_changed = false;
+bool firstWrite = false;
+
// Store settings in the last two pages
// Flash pages must be erased before writing, so keep track.
-bool firstWrite = false;
uint32_t pageBase = EEPROM_START_ADDRESS;
-bool PersistentStore::access_start() {
- firstWrite = true;
- return true;
+inline uint16 CheckPage(uint32 pageBase)
+{
+ uint32 pageEnd = pageBase + (uint32)EEPROM_PAGE_SIZE;
+
+ // Page Status not EEPROM_ERASED
+ if ((*(__IO uint16*)pageBase) != EEPROM_ERASED)
+ return EEPROM_BAD_FLASH;
+ for(pageBase += 4; pageBase < pageEnd; pageBase += 4)
+ if ((*(__IO uint32*)pageBase) != 0xFFFFFFFF) // Verify if slot is empty
+ return EEPROM_BAD_FLASH;
+ return EEPROM_OK;
}
-bool PersistentStore::access_finish() {
- FLASH_Lock();
- firstWrite = false;
+bool PersistentStore::access_start() {
+
+ if (CheckPage(EEPROM_PAGE0_BASE) != EEPROM_OK || CheckPage(EEPROM_PAGE1_BASE) != EEPROM_OK) {
+ for (uint32_t i = 0; i < EEPROM_SIZE; i++) {
+ uint8* accessPoint = (uint8*)pageBase + i;
+ uint8_t c = *accessPoint;
+ ram_eeprom[i] = c;
+ }
+ } else {
+ for (int i = 0; i < EEPROM_SIZE; i++) {
+ ram_eeprom[i] = EEPROM_ERASE;
+ }
+ }
+ eeprom_changed = false;
return true;
}
-bool PersistentStore::write_data(int &pos, const uint8_t *value, const size_t size, uint16_t *crc) {
+bool PersistentStore::access_finish() {
FLASH_Status status;
+ if (eeprom_changed) {
- if (firstWrite) {
FLASH_Unlock();
+
status = FLASH_ErasePage(EEPROM_PAGE0_BASE);
if (status != FLASH_COMPLETE) return true;
status = FLASH_ErasePage(EEPROM_PAGE1_BASE);
- if (status != FLASH_COMPLETE) return true;
- firstWrite = false;
+ if (status != FLASH_COMPLETE) return true;
}
- // First write full words
int i = 0;
- int wordsToWrite = size / sizeof(uint16_t);
- uint16_t* wordBuffer = (uint16_t *)value;
+ int wordsToWrite = EEPROM_SIZE / sizeof(uint16_t);
+ uint16_t* wordBuffer = (uint16_t *)ram_eeprom;
while (wordsToWrite) {
- status = FLASH_ProgramHalfWord(pageBase + pos + (i * 2), wordBuffer[i]);
+ status = FLASH_ProgramHalfWord(pageBase + (i *2), wordBuffer[i]);
if (status != FLASH_COMPLETE) return true;
wordsToWrite--;
i++;
}
- // Now, write any remaining single byte
- const uint16_t odd = size & 1;
- if (odd) {
- uint16_t temp = value[size - 1];
- status = FLASH_ProgramHalfWord(pageBase + pos + i, temp);
- if (status != FLASH_COMPLETE) return true;
- }
+ FLASH_Lock();
+
+ eeprom_changed = false;
+ return true;
+}
+
+bool PersistentStore::read_data(int &pos, uint8_t* value, size_t size, uint16_t *crc, const bool writing/*=true*/) {
+
+ const uint8_t * const buff = writing ? &value[0] : &ram_eeprom[pos];
+ if (writing) for (size_t i = 0; i < size; i++) value[i] = ram_eeprom[pos + i];
+ crc16(crc, buff, size);
+ pos += size;
- crc16(crc, value, size);
- pos += size + odd;
return false;
}
-bool PersistentStore::read_data(int &pos, uint8_t* value, const size_t size, uint16_t *crc, const bool writing/*=true*/) {
- for (uint16_t i = 0; i < size; i++) {
- byte* accessPoint = (byte*)(pageBase + pos + i);
- uint8_t c = *accessPoint;
- if (writing) value[i] = c;
- crc16(crc, &c, 1);
- }
- pos += ((size + 1) & ~1); // i.e., size+(size&1), round up odd values
+bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) {
+
+ for (size_t i = 0; i < size; i++) ram_eeprom[pos + i] = value[i];
+ eeprom_changed = true;
+ crc16(crc, value, size);
+ pos += size;
+
return false;
}
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_sdcard.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_sdcard.cpp
index 2b184cd..d33c000 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_sdcard.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/persistent_store_sdcard.cpp
@@ -29,7 +29,7 @@
#include "../../inc/MarlinConfig.h"
-#if ENABLED(EEPROM_SETTINGS) && DISABLED(FLASH_EEPROM_EMULATION)
+#if ENABLED(EEPROM_SETTINGS) && DISABLED(FLASH_EEPROM_EMULATION) && DISABLED(I2C_EEPROM)
#include "../shared/persistent_store_api.h"
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/u8g_com_stm32duino_fsmc.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/u8g_com_stm32duino_fsmc.cpp
index 1d2153d..318cc92 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/u8g_com_stm32duino_fsmc.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/u8g_com_stm32duino_fsmc.cpp
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -26,27 +26,40 @@
* Communication interface for FSMC
*/
-#if defined(ARDUINO_ARCH_STM32F1) && (defined(STM32_HIGH_DENSITY) || defined(STM32_XL_DENSITY))
-
#include "../../inc/MarlinConfig.h"
#if HAS_GRAPHICAL_LCD
+#if defined(ARDUINO_ARCH_STM32F1) && (defined(STM32_HIGH_DENSITY) || defined(STM32_XL_DENSITY))
#include "U8glib.h"
#include "libmaple/fsmc.h"
#include "libmaple/gpio.h"
+#include "libmaple/dma.h"
#include "boards.h"
-#define LCD_READ_ID 0x04 /* Read display identification information */
+struct LCD_IO {
+ uint32_t id;
+ void (*writeRegister)(uint16_t reg);
+ uint16_t (*readData)(void);
+ void (*writeData)(uint16_t data);
+ void (*writeMultiple)(uint16_t data, uint32_t count);
+ void (*writeSequence)(uint16_t *data, uint16_t length);
+ void (*setWindow)(uint16_t Xmin, uint16_t Ymin, uint16_t Xmax, uint16_t Ymax);
+};
/* Timing configuration */
#define FSMC_ADDRESS_SETUP_TIME 15 // AddressSetupTime
#define FSMC_DATA_SETUP_TIME 15 // DataSetupTime
+#define FSMC_DMA_DEV DMA2
+#define FSMC_DMA_CHANNEL DMA_CH4
+
void LCD_IO_Init(uint8_t cs, uint8_t rs);
-void LCD_IO_WriteData(uint16_t RegValue);
-void LCD_IO_WriteReg(uint8_t Reg);
-uint32_t LCD_IO_ReadData(uint16_t RegValue, uint8_t ReadSize);
+void LCD_IO_WriteReg(uint16_t reg);
+uint16_t LCD_IO_ReadData(void);
+void LCD_IO_WriteData(uint16_t data);
+void LCD_IO_WriteMultiple(uint16_t data, uint32_t count);
+void LCD_IO_WriteSequence(uint16_t *data, uint16_t length);
static uint8_t msgInitCount = 2; // Ignore all messages until 2nd U8G_COM_MSG_INIT
@@ -56,43 +69,31 @@ uint8_t u8g_com_stm32duino_fsmc_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, voi
if (msgInitCount) return -1;
}
- static uint8_t isCommand;
-
- switch (msg) {
+ switch(msg) {
case U8G_COM_MSG_STOP:
break;
case U8G_COM_MSG_INIT:
u8g_SetPIOutput(u8g, U8G_PI_RESET);
+ dma_init(FSMC_DMA_DEV);
+ dma_disable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
+ dma_set_priority(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, DMA_PRIORITY_HIGH);
+
LCD_IO_Init(u8g->pin_list[U8G_PI_CS], u8g->pin_list[U8G_PI_A0]);
u8g_Delay(100);
- if (arg_ptr != NULL)
- *((uint32_t *)arg_ptr) = LCD_IO_ReadData(LCD_READ_ID, 3);
-
- isCommand = 0;
- break;
-
- case U8G_COM_MSG_ADDRESS: // define cmd (arg_val = 0) or data mode (arg_val = 1)
- isCommand = arg_val == 0 ? 1 : 0;
+ if (arg_ptr != NULL) {
+ ((LCD_IO *)arg_ptr)->writeRegister = LCD_IO_WriteReg;
+ ((LCD_IO *)arg_ptr)->readData = LCD_IO_ReadData;
+ ((LCD_IO *)arg_ptr)->writeData = LCD_IO_WriteData;
+ ((LCD_IO *)arg_ptr)->writeMultiple = LCD_IO_WriteMultiple;
+ ((LCD_IO *)arg_ptr)->writeSequence = LCD_IO_WriteSequence;
+ }
break;
case U8G_COM_MSG_RESET:
u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val);
break;
-
- case U8G_COM_MSG_WRITE_BYTE:
- if (isCommand)
- LCD_IO_WriteReg(arg_val);
- else
- LCD_IO_WriteData((uint16_t)arg_val);
- break;
-
- case U8G_COM_MSG_WRITE_SEQ:
-
- for (uint8_t i = 0; i < arg_val; i += 2)
- LCD_IO_WriteData(*(uint16_t *)(((uint32_t)arg_ptr) + i));
- break;
}
return 1;
}
@@ -107,7 +108,7 @@ __attribute__((always_inline)) __STATIC_INLINE void __DSB(void) {
__ASM volatile ("dsb 0xF":::"memory");
}
-#define FSMC_CS_NE1 PD7
+#define FSMC_CS_NE1 PD7 //LQFP100 pin 88
#ifdef STM32_XL_DENSITY
#define FSMC_CS_NE2 PG9
@@ -132,14 +133,14 @@ __attribute__((always_inline)) __STATIC_INLINE void __DSB(void) {
#define FSMC_RS_A15 PG5
#endif
-#define FSMC_RS_A16 PD11
-#define FSMC_RS_A17 PD12
-#define FSMC_RS_A18 PD13
-#define FSMC_RS_A19 PE3
-#define FSMC_RS_A20 PE4
-#define FSMC_RS_A21 PE5
-#define FSMC_RS_A22 PE6
-#define FSMC_RS_A23 PE2
+#define FSMC_RS_A16 PD11
+#define FSMC_RS_A17 PD12
+#define FSMC_RS_A18 PD13
+#define FSMC_RS_A19 PE3
+#define FSMC_RS_A20 PE4
+#define FSMC_RS_A21 PE5
+#define FSMC_RS_A22 PE6
+#define FSMC_RS_A23 PE2
#ifdef STM32_XL_DENSITY
#define FSMC_RS_A24 PG13
@@ -161,7 +162,7 @@ void LCD_IO_Init(uint8_t cs, uint8_t rs) {
if (fsmcInit) return;
fsmcInit = 1;
- switch (cs) {
+ switch(cs) {
case FSMC_CS_NE1: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION1; break;
#ifdef STM32_XL_DENSITY
case FSMC_CS_NE2: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION2; break;
@@ -173,7 +174,7 @@ void LCD_IO_Init(uint8_t cs, uint8_t rs) {
#define _ORADDR(N) controllerAddress |= (_BV32(N) - 2)
- switch (rs) {
+ switch(rs) {
#ifdef STM32_XL_DENSITY
case FSMC_RS_A0: _ORADDR( 1); break;
case FSMC_RS_A1: _ORADDR( 2); break;
@@ -240,31 +241,43 @@ void LCD_IO_Init(uint8_t cs, uint8_t rs) {
LCD = (LCD_CONTROLLER_TypeDef*)controllerAddress;
}
-void LCD_IO_WriteData(uint16_t RegValue) {
- LCD->RAM = RegValue;
- __DSB();
+void LCD_IO_WriteReg(uint16_t reg) {
+ LCD->REG = reg;
+ __DSB();
}
-void LCD_IO_WriteReg(uint8_t Reg) {
- LCD->REG = (uint16_t)Reg;
- __DSB();
+uint16_t LCD_IO_ReadData(void) {
+ return LCD->RAM;
}
-uint32_t LCD_IO_ReadData(uint16_t RegValue, uint8_t ReadSize) {
- volatile uint32_t data;
- LCD->REG = (uint16_t)RegValue;
+void LCD_IO_WriteData(uint16_t data) {
+ LCD->RAM = data;
__DSB();
+}
- data = LCD->RAM; // dummy read
- data = LCD->RAM & 0x00FF;
+void LCD_IO_WriteMultiple(uint16_t data, uint32_t count) {
+ while (count > 0) {
+ dma_setup_transfer(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, &data, DMA_SIZE_16BITS, &LCD->RAM, DMA_SIZE_16BITS, DMA_MEM_2_MEM);
+ dma_set_num_transfers(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, count > 65535 ? 65535 : count);
+ dma_clear_isr_bits(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
+ dma_enable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
- while (--ReadSize) {
- data <<= 8;
- data |= (LCD->RAM & 0x00FF);
+ while ((dma_get_isr_bits(FSMC_DMA_DEV, FSMC_DMA_CHANNEL) & 0x0A) == 0) {};
+ dma_disable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
+
+ count = count > 65535 ? count - 65535 : 0;
}
- return (uint32_t)data;
}
-#endif // HAS_GRAPHICAL_LCD
+void LCD_IO_WriteSequence(uint16_t *data, uint16_t length) {
+ dma_setup_transfer(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, data, DMA_SIZE_16BITS, &LCD->RAM, DMA_SIZE_16BITS, DMA_MEM_2_MEM | DMA_PINC_MODE);
+ dma_set_num_transfers(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, length);
+ dma_clear_isr_bits(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
+ dma_enable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
+
+ while ((dma_get_isr_bits(FSMC_DMA_DEV, FSMC_DMA_CHANNEL) & 0x0A) == 0) {};
+ dma_disable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL);
+}
#endif // ARDUINO_ARCH_STM32F1 && (STM32_HIGH_DENSITY || STM32_XL_DENSITY)
+#endif // HAS_GRAPHICAL_LCD
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.cpp
index 0cbfb7f..7ecf3fd 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.cpp
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.cpp
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -33,13 +33,6 @@
#include <libmaple/iwdg.h>
#include "watchdog_STM32F1.h"
-void watchdog_reset() {
- #if PIN_EXISTS(LED)
- TOGGLE(LED_PIN); // heartbeat indicator
- #endif
- iwdg_feed();
-}
-
void watchdogSetup(void) {
// do whatever. don't remove this function.
}
diff --git a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.h
index db62d44..d34efdc 100644
--- a/marlin-original-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.h
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/watchdog_STM32F1.h
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -27,6 +27,8 @@
#include <libmaple/iwdg.h>
+#include "../../inc/MarlinConfig.h"
+
/**
* The watchdog clock is 40Khz. We need a 4 seconds interval, so use a /256 preescaler and
* 625 reload value (counts down to 0)
@@ -41,4 +43,9 @@ void watchdog_init();
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or STM32F1 will reset.
-void watchdog_reset();
+inline void watchdog_reset() {
+ #if PIN_EXISTS(LED)
+ TOGGLE(LED_PIN); // heart beat indicator
+ #endif
+ iwdg_feed();
+}
diff --git a/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/xpt2046.cpp b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/xpt2046.cpp
new file mode 100644
index 0000000..91dfeb0
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/xpt2046.cpp
@@ -0,0 +1,64 @@
+//#include <Arduino.h>
+#include <SPI.h>
+
+#include "xpt2046.h"
+
+static SPISettings spiConfig;
+static uint32_t timeout = 0;
+
+extern int8_t encoderDiff;
+
+uint8_t xpt2046_read_buttons() {
+ uint16_t x, y;
+
+ if (timeout > millis()) return 0;
+ timeout = millis() + 250;
+
+ spiConfig = SPISettings(XPT2046_SPI_CLOCK, MSBFIRST, SPI_MODE0);
+ if (!getTouchPoint(&x, &y)) return 0;
+
+ x = (uint16_t)((((int32_t)x * (int32_t)XPT2046_X_CALIBRATION) >> 16) + XPT2046_X_OFFSET);
+ y = (uint16_t)((((int32_t)y * (int32_t)XPT2046_Y_CALIBRATION) >> 16) + XPT2046_Y_OFFSET);
+
+ if (y < 185 || y > 224) return 0;
+ if (x > 20 & x < 99) encoderDiff = - ENCODER_STEPS_PER_MENU_ITEM * ENCODER_PULSES_PER_STEP;
+ if (x > 120 & x < 199) encoderDiff = ENCODER_STEPS_PER_MENU_ITEM * ENCODER_PULSES_PER_STEP;
+ if (x > 220 & x < 299) return EN_C;
+ return 0;
+}
+
+uint16_t getTouchCoordinate(uint8_t coordinate) {
+ coordinate |= XPT2046_CONTROL | XPT2046_DFR_MODE;
+
+ OUT_WRITE(TOUCH_CS, LOW);
+ SPI.beginTransaction(spiConfig);
+
+ uint16_t data[3], delta[3];
+
+ SPI.transfer(coordinate);
+ for (uint32_t i = 0; i < 3 ; i++) {
+ data[i] = ((uint16_t) SPI.transfer(0x00)) << 4;
+ data[i] |= ((uint16_t) SPI.transfer(coordinate)) >> 4;
+ }
+ SPI.transfer(0x00);
+ SPI.transfer(0x00);
+ SPI.transfer(0x00);
+
+ SPI.endTransaction();
+ WRITE(TOUCH_CS, HIGH);
+
+ delta[0] = data[0] > data[1] ? data[0] - data [1] : data[1] - data [0];
+ delta[1] = data[0] > data[2] ? data[0] - data [2] : data[2] - data [0];
+ delta[2] = data[1] > data[2] ? data[1] - data [2] : data[2] - data [1];
+
+ if (delta[0] <= delta[1] && delta[0] <= delta[2]) return (data[0] + data [1]) >> 1;
+ if (delta[1] <= delta[2]) return (data[0] + data [2]) >> 1;
+ return (data[1] + data [2]) >> 1;
+}
+
+bool getTouchPoint(uint16_t *x, uint16_t *y) {
+ if (!isTouched()) return false;
+ *x = getTouchCoordinate(XPT2046_X);
+ *y = getTouchCoordinate(XPT2046_Y);
+ return isTouched();
+}
diff --git a/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/xpt2046.h b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/xpt2046.h
new file mode 100644
index 0000000..ce868ee
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/HAL/HAL_STM32F1/xpt2046.h
@@ -0,0 +1,58 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "HAL.h"
+#include "../../lcd/ultralcd.h"
+
+#define XPT2046_DFR_MODE 0x00
+#define XPT2046_SER_MODE 0x04
+#define XPT2046_CONTROL 0x80
+
+#define XPT2046_X 0x10
+#define XPT2046_Z1 0x30
+#define XPT2046_Z2 0x40
+#define XPT2046_Y 0x50
+
+#define XPT2046_SPI_CLOCK SPI_CLOCK_DIV2
+
+#define XPT2046_Z1_TRESHHOLD 10
+
+/* MKS Robin TFT v2.0 */
+#define XPT2046_X_CALIBRATION 12149
+#define XPT2046_X_OFFSET -35
+#define XPT2046_Y_CALIBRATION -8746
+#define XPT2046_Y_OFFSET 256
+
+/* MKS Robin TFT v1.1 */
+//#define XPT2046_X_CALIBRATION -11792
+//#define XPT2046_X_OFFSET 342
+//#define XPT2046_Y_CALIBRATION 8947
+//#define XPT2046_Y_OFFSET -19
+
+uint8_t xpt2046_read_buttons();
+uint16_t getTouchCoordinate(uint8_t coordinate);
+bool getTouchPoint(uint16_t *x, uint16_t *y);
+
+inline bool isTouched() { return getTouchCoordinate(XPT2046_Z1) >= XPT2046_Z1_TRESHHOLD; }
+inline void waitForRelease(void) { while (isTouched()) {}; }
+inline void waitForTouch(uint16_t *x, uint16_t *y) { while(!getTouchPoint(x, y)) {}; }
diff --git a/marlin-original-clean/Marlin/src/Marlin.cpp b/marlin-qq-clean/Marlin/src/Marlin.cpp
index e612b0d..5cf65cd 100644
--- a/marlin-original-clean/Marlin/src/Marlin.cpp
+++ b/marlin-qq-clean/Marlin/src/Marlin.cpp
@@ -939,6 +939,15 @@ void setup() {
queue_setup();
+ // UI must be initialized before EEPROM
+ // (because EEPROM code calls the UI).
+ ui.init();
+ ui.reset_status();
+
+ #if HAS_SPI_LCD && ENABLED(SHOW_BOOTSCREEN)
+ ui.show_bootscreen();
+ #endif
+
#if ENABLED(SDIO_SUPPORT) && SD_DETECT_PIN == -1
// Auto-mount the SD for EEPROM.dat emulation
if (!card.isDetected()) card.initsd();
@@ -1044,13 +1053,6 @@ void setup() {
fanmux_init();
#endif
- ui.init();
- ui.reset_status();
-
- #if HAS_SPI_LCD && ENABLED(SHOW_BOOTSCREEN)
- ui.show_bootscreen();
- #endif
-
#if ENABLED(MIXING_EXTRUDER)
mixer.init();
#endif
diff --git a/marlin-original-clean/Marlin/src/core/boards.h b/marlin-qq-clean/Marlin/src/core/boards.h
index 4a7fb64..8581169 100644
--- a/marlin-original-clean/Marlin/src/core/boards.h
+++ b/marlin-qq-clean/Marlin/src/core/boards.h
@@ -244,6 +244,7 @@
#define BOARD_GTM32_PRO_VB 1805 // STM32f103VET6 controller
#define BOARD_MORPHEUS 1806 // STM32F103C8/STM32F103CB Libmaple based stm32f1 controller
#define BOARD_MKS_ROBIN 1808 // MKS Robin / STM32F103ZET6
+#define BOARD_MKS_ROBIN_MINI 1812 // MKS Robin / STM32F103VET6
//
// STM32 ARM Cortex-M4F
diff --git a/marlin-qq-clean/Marlin/src/feature/backlash.cpp b/marlin-qq-clean/Marlin/src/feature/backlash.cpp
new file mode 100644
index 0000000..3254fb3
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/feature/backlash.cpp
@@ -0,0 +1,139 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2019 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../Marlin.h"
+
+#if ENABLED(BACKLASH_COMPENSATION)
+
+#include "backlash.h"
+#include "../module/planner.h"
+
+#if ENABLED(BACKLASH_GCODE)
+ uint8_t Backlash::correction = (BACKLASH_CORRECTION) * 0xFF;
+ #ifdef BACKLASH_DISTANCE_MM
+ float Backlash::distance_mm[XYZ] = BACKLASH_DISTANCE_MM;
+ #endif
+ #ifdef BACKLASH_SMOOTHING_MM
+ float Backlash::smoothing_mm = BACKLASH_SMOOTHING_MM;
+ #endif
+#endif
+
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+ float Backlash::measured_mm[XYZ] = { 0 };
+ uint8_t Backlash::measured_count[XYZ] = { 0 };
+#endif
+
+Backlash backlash;
+
+/**
+ * To minimize seams in the printed part, backlash correction only adds
+ * steps to the current segment (instead of creating a new segment, which
+ * causes discontinuities and print artifacts).
+ *
+ * With a non-zero BACKLASH_SMOOTHING_MM value the backlash correction is
+ * spread over multiple segments, smoothing out artifacts even more.
+ */
+
+void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block) {
+ static uint8_t last_direction_bits;
+ uint8_t changed_dir = last_direction_bits ^ dm;
+ // Ignore direction change if no steps are taken in that direction
+ if (da == 0) CBI(changed_dir, X_AXIS);
+ if (db == 0) CBI(changed_dir, Y_AXIS);
+ if (dc == 0) CBI(changed_dir, Z_AXIS);
+ last_direction_bits ^= changed_dir;
+
+ if (correction == 0) return;
+
+ #ifdef BACKLASH_SMOOTHING_MM
+ // The segment proportion is a value greater than 0.0 indicating how much residual_error
+ // is corrected for in this segment. The contribution is based on segment length and the
+ // smoothing distance. Since the computation of this proportion involves a floating point
+ // division, defer computation until needed.
+ float segment_proportion = 0;
+
+ // Residual error carried forward across multiple segments, so correction can be applied
+ // to segments where there is no direction change.
+ static int32_t residual_error[XYZ] = { 0 };
+ #else
+ // No leftover residual error from segment to segment
+ int32_t residual_error[XYZ] = { 0 };
+ // No direction change, no correction.
+ if (!changed_dir) return;
+ #endif
+
+ const float f_corr = float(correction) / 255.0f;
+
+ LOOP_XYZ(axis) {
+ if (distance_mm[axis]) {
+ const bool reversing = TEST(dm,axis);
+
+ // When an axis changes direction, add axis backlash to the residual error
+ if (TEST(changed_dir, axis))
+ residual_error[axis] += (reversing ? -f_corr : f_corr) * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
+
+ // Decide how much of the residual error to correct in this segment
+ int32_t error_correction = residual_error[axis];
+ #ifdef BACKLASH_SMOOTHING_MM
+ if (error_correction && smoothing_mm != 0) {
+ // Take up a portion of the residual_error in this segment, but only when
+ // the current segment travels in the same direction as the correction
+ if (reversing == (error_correction < 0)) {
+ if (segment_proportion == 0)
+ segment_proportion = MIN(1.0f, block->millimeters / smoothing_mm);
+ error_correction = ceil(segment_proportion * error_correction);
+ }
+ else
+ error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps
+ }
+ #endif
+ // Making a correction reduces the residual error and modifies delta_mm
+ if (error_correction) {
+ block->steps[axis] += ABS(error_correction);
+ residual_error[axis] -= error_correction;
+ }
+ }
+ }
+}
+
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+ #if USES_Z_MIN_PROBE_ENDSTOP
+ #define TEST_PROBE_PIN (READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING)
+ #else
+ #define TEST_PROBE_PIN (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)
+ #endif
+
+ // Measure Z backlash by raising nozzle in increments until probe deactivates
+ void Backlash::measure_with_probe() {
+ if (measured_count[Z_AXIS] == 255) return;
+
+ float start_height = current_position[Z_AXIS];
+ while (current_position[Z_AXIS] < (start_height + BACKLASH_MEASUREMENT_LIMIT) && TEST_PROBE_PIN)
+ do_blocking_move_to_z(current_position[Z_AXIS] + BACKLASH_MEASUREMENT_RESOLUTION, MMM_TO_MMS(BACKLASH_MEASUREMENT_FEEDRATE));
+
+ // The backlash from all probe points is averaged, so count the number of measurements
+ measured_mm[Z_AXIS] += current_position[Z_AXIS] - start_height;
+ measured_count[Z_AXIS]++;
+ }
+#endif
+
+#endif // BACKLASH_COMPENSATION
diff --git a/marlin-qq-clean/Marlin/src/feature/backlash.h b/marlin-qq-clean/Marlin/src/feature/backlash.h
new file mode 100644
index 0000000..c35675f
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/feature/backlash.h
@@ -0,0 +1,88 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2019 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h"
+#include "../module/planner.h"
+
+class Backlash {
+public:
+ #if ENABLED(BACKLASH_GCODE)
+ static uint8_t correction;
+ #ifdef BACKLASH_DISTANCE_MM
+ static float distance_mm[XYZ];
+ #endif
+ #ifdef BACKLASH_SMOOTHING_MM
+ static float smoothing_mm;
+ #endif
+ static inline void set_correction(const float &v) { correction = MAX(0, MIN(1.0, v)) * all_on; }
+ static inline float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; }
+ #elif ENABLED(BACKLASH_COMPENSATION)
+ static constexpr uint8_t correction = (BACKLASH_CORRECTION) * 0xFF;
+ #ifdef BACKLASH_DISTANCE_MM
+ static constexpr float distance_mm[XYZ] = BACKLASH_DISTANCE_MM;
+ #endif
+ #ifdef BACKLASH_SMOOTHING_MM
+ static constexpr float smoothing_mm = BACKLASH_SMOOTHING_MM;
+ #endif
+ static inline void set_correction(float) { }
+ static inline float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; }
+ #else
+ static constexpr uint8_t correction = 0;
+ static inline void set_correction(float) { }
+ static inline float get_correction() { return 0; }
+ #endif
+
+ #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+ private:
+ static float measured_mm[XYZ];
+ static uint8_t measured_count[XYZ];
+ public:
+ static void measure_with_probe();
+ #endif
+
+ static inline float get_measurement(const uint8_t e) {
+ // Return the measurement averaged over all readings
+ return (
+ #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+ measured_count[e] > 0 ? measured_mm[e] / measured_count[e] :
+ #endif
+ 0
+ );
+ }
+
+ static inline bool has_measurement(const uint8_t e) {
+ return (false
+ #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+ || (measured_count[e] > 0)
+ #endif
+ );
+ }
+
+ static inline bool has_any_measurement() {
+ return has_measurement(X_AXIS) || has_measurement(Y_AXIS) || has_measurement(Z_AXIS);
+ }
+
+ void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block);
+};
+
+extern Backlash backlash;
diff --git a/marlin-original-clean/Marlin/src/feature/runout.cpp b/marlin-qq-clean/Marlin/src/feature/runout.cpp
index 9395dec..e319a7c 100644
--- a/marlin-original-clean/Marlin/src/feature/runout.cpp
+++ b/marlin-qq-clean/Marlin/src/feature/runout.cpp
@@ -51,7 +51,7 @@ void FilamentSensorBase::filament_present(const uint8_t extruder) {
uint8_t FilamentSensorEncoder::motion_detected;
#endif
-#if FILAMENT_RUNOUT_DISTANCE_MM > 0
+#ifdef FILAMENT_RUNOUT_DISTANCE_MM
float RunoutResponseDelayed::runout_distance_mm = FILAMENT_RUNOUT_DISTANCE_MM;
volatile float RunoutResponseDelayed::runout_mm_countdown[EXTRUDERS];
#else
diff --git a/marlin-original-clean/Marlin/src/feature/runout.h b/marlin-qq-clean/Marlin/src/feature/runout.h
index 3f1e5fa..eccc7ab 100644
--- a/marlin-original-clean/Marlin/src/feature/runout.h
+++ b/marlin-qq-clean/Marlin/src/feature/runout.h
@@ -78,6 +78,11 @@ class TFilamentMonitor : public FilamentMonitorBase {
response.filament_present(extruder);
}
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ static inline float& runout_distance() { return response.runout_distance_mm; }
+ static inline void set_runout_distance(const float &mm) { response.runout_distance_mm = mm; }
+ #endif
+
// Handle a block completion. RunoutResponseDelayed uses this to
// add up the length of filament moved while the filament is out.
static inline void block_completed(const block_t* const b) {
@@ -90,13 +95,13 @@ class TFilamentMonitor : public FilamentMonitorBase {
// Give the response a chance to update its counter.
static inline void run() {
if (enabled && !filament_ran_out && (IS_SD_PRINTING() || print_job_timer.isRunning() || did_pause_print)) {
- #if FILAMENT_RUNOUT_DISTANCE_MM > 0
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
cli(); // Prevent RunoutResponseDelayed::block_completed from accumulating here
#endif
response.run();
sensor.run();
const bool ran_out = response.has_run_out();
- #if FILAMENT_RUNOUT_DISTANCE_MM > 0
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
sei();
#endif
if (ran_out) {
@@ -272,7 +277,7 @@ class FilamentSensorBase {
/********************************* RESPONSE TYPE *********************************/
-#if FILAMENT_RUNOUT_DISTANCE_MM > 0
+#ifdef FILAMENT_RUNOUT_DISTANCE_MM
// RunoutResponseDelayed triggers a runout event only if the length
// of filament specified by FILAMENT_RUNOUT_DISTANCE_MM has been fed
@@ -347,11 +352,12 @@ class FilamentSensorBase {
/********************************* TEMPLATE SPECIALIZATION *********************************/
typedef TFilamentMonitor<
- #if FILAMENT_RUNOUT_DISTANCE_MM > 0
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ RunoutResponseDelayed,
#if ENABLED(FILAMENT_MOTION_SENSOR)
- RunoutResponseDelayed, FilamentSensorEncoder
+ FilamentSensorEncoder
#else
- RunoutResponseDelayed, FilamentSensorSwitch
+ FilamentSensorSwitch
#endif
#else
RunoutResponseDebounced, FilamentSensorSwitch
diff --git a/marlin-original-clean/Marlin/src/gcode/calibrate/G425.cpp b/marlin-qq-clean/Marlin/src/gcode/calibrate/G425.cpp
index 1174fc3..03e4c4e 100644
--- a/marlin-original-clean/Marlin/src/gcode/calibrate/G425.cpp
+++ b/marlin-qq-clean/Marlin/src/gcode/calibrate/G425.cpp
@@ -31,6 +31,7 @@
#include "../../module/tool_change.h"
#include "../../module/endstops.h"
#include "../../feature/bedlevel/bedlevel.h"
+#include "../../feature/backlash.h"
/**
@@ -55,11 +56,6 @@
#define HAS_X_CENTER BOTH(CALIBRATION_MEASURE_LEFT, CALIBRATION_MEASURE_RIGHT)
#define HAS_Y_CENTER BOTH(CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK)
-#if ENABLED(BACKLASH_GCODE)
- extern float backlash_distance_mm[], backlash_smoothing_mm;
- extern uint8_t backlash_correction;
-#endif
-
enum side_t : uint8_t { TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES };
struct measurements_t {
@@ -79,13 +75,13 @@ struct measurements_t {
#define TEMPORARY_SOFT_ENDSTOP_STATE(enable) REMEMBER(tes, soft_endstops_enabled, enable);
#if ENABLED(BACKLASH_GCODE)
- #define TEMPORARY_BACKLASH_CORRECTION(value) REMEMBER(tbst, backlash_correction, value)
+ #define TEMPORARY_BACKLASH_CORRECTION(value) REMEMBER(tbst, backlash.correction, value)
#else
#define TEMPORARY_BACKLASH_CORRECTION(value)
#endif
#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
- #define TEMPORARY_BACKLASH_SMOOTHING(value) REMEMBER(tbsm, backlash_smoothing_mm, value)
+ #define TEMPORARY_BACKLASH_SMOOTHING(value) REMEMBER(tbsm, backlash.smoothing_mm, value)
#else
#define TEMPORARY_BACKLASH_SMOOTHING(value)
#endif
@@ -454,22 +450,22 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
#if ENABLED(BACKLASH_GCODE)
#if HAS_X_CENTER
- backlash_distance_mm[X_AXIS] = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
+ backlash.distance_mm[X_AXIS] = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
#elif ENABLED(CALIBRATION_MEASURE_LEFT)
- backlash_distance_mm[X_AXIS] = m.backlash[LEFT];
+ backlash.distance_mm[X_AXIS] = m.backlash[LEFT];
#elif ENABLED(CALIBRATION_MEASURE_RIGHT)
- backlash_distance_mm[X_AXIS] = m.backlash[RIGHT];
+ backlash.distance_mm[X_AXIS] = m.backlash[RIGHT];
#endif
#if HAS_Y_CENTER
- backlash_distance_mm[Y_AXIS] = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
+ backlash.distance_mm[Y_AXIS] = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
#elif ENABLED(CALIBRATION_MEASURE_FRONT)
- backlash_distance_mm[Y_AXIS] = m.backlash[FRONT];
+ backlash.distance_mm[Y_AXIS] = m.backlash[FRONT];
#elif ENABLED(CALIBRATION_MEASURE_BACK)
- backlash_distance_mm[Y_AXIS] = m.backlash[BACK];
+ backlash.distance_mm[Y_AXIS] = m.backlash[BACK];
#endif
- backlash_distance_mm[Z_AXIS] = m.backlash[TOP];
+ backlash.distance_mm[Z_AXIS] = m.backlash[TOP];
#endif
}
diff --git a/marlin-original-clean/Marlin/src/gcode/calibrate/M425.cpp b/marlin-qq-clean/Marlin/src/gcode/calibrate/M425.cpp
index e51c930..74b38f5 100644
--- a/marlin-original-clean/Marlin/src/gcode/calibrate/M425.cpp
+++ b/marlin-qq-clean/Marlin/src/gcode/calibrate/M425.cpp
@@ -24,20 +24,9 @@
#if ENABLED(BACKLASH_GCODE)
+#include "../../feature/backlash.h"
#include "../../module/planner.h"
-float backlash_distance_mm[XYZ] = BACKLASH_DISTANCE_MM;
-uint8_t backlash_correction = BACKLASH_CORRECTION * all_on;
-
-#ifdef BACKLASH_SMOOTHING_MM
- float backlash_smoothing_mm = BACKLASH_SMOOTHING_MM;
-#endif
-
-#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
- float backlash_measured_mm[XYZ] = { 0 };
- uint8_t backlash_measured_num[XYZ] = { 0 };
-#endif
-
#include "../gcode.h"
/**
@@ -60,59 +49,52 @@ void GcodeSuite::M425() {
LOOP_XYZ(i) {
if (parser.seen(axis_codes[i])) {
planner.synchronize();
- const float measured_backlash = (
- #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
- backlash_measured_num[i] > 0 ? backlash_measured_mm[i] / backlash_measured_num[i] : 0
- #else
- 0
- #endif
- );
- backlash_distance_mm[i] = parser.has_value() ? parser.value_linear_units() : measured_backlash;
+ backlash.distance_mm[i] = parser.has_value() ? parser.value_linear_units() : backlash.get_measurement(i);
noArgs = false;
}
}
if (parser.seen('F')) {
planner.synchronize();
- backlash_correction = MAX(0, MIN(1.0, parser.value_float())) * all_on;
+ backlash.set_correction(parser.value_float());
noArgs = false;
}
#ifdef BACKLASH_SMOOTHING_MM
if (parser.seen('S')) {
planner.synchronize();
- backlash_smoothing_mm = parser.value_linear_units();
+ backlash.smoothing_mm = parser.value_linear_units();
noArgs = false;
}
#endif
if (noArgs) {
- SERIAL_ECHOPGM("Backlash correction is ");
- if (!backlash_correction) SERIAL_ECHOPGM("in");
+ SERIAL_ECHOPGM("Backlash Correction ");
+ if (!backlash.correction) SERIAL_ECHOPGM("in");
SERIAL_ECHOLNPGM("active:");
- SERIAL_ECHOLNPAIR(" Correction Amount/Fade-out: F", float(ui8_to_percent(backlash_correction)) / 100, " (F1.0 = full, F0.0 = none)");
+ SERIAL_ECHOLNPAIR(" Correction Amount/Fade-out: F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)");
SERIAL_ECHOPGM(" Backlash Distance (mm): ");
LOOP_XYZ(a) {
SERIAL_CHAR(' ');
SERIAL_CHAR(axis_codes[a]);
- SERIAL_ECHO(backlash_distance_mm[a]);
+ SERIAL_ECHO(backlash.distance_mm[a]);
SERIAL_EOL();
}
#ifdef BACKLASH_SMOOTHING_MM
- SERIAL_ECHOLNPAIR(" Smoothing (mm): S", backlash_smoothing_mm);
+ SERIAL_ECHOLNPAIR(" Smoothing (mm): S", backlash.smoothing_mm);
#endif
#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
SERIAL_ECHOPGM(" Average measured backlash (mm):");
- LOOP_XYZ(a) {
- if (backlash_measured_num[a] > 0) {
+ if (backlash.has_any_measurement()) {
+ LOOP_XYZ(a) if (backlash.has_measurement(a)) {
SERIAL_CHAR(' ');
SERIAL_CHAR(axis_codes[a]);
- SERIAL_ECHO(backlash_measured_mm[a] / backlash_measured_num[a]);
+ SERIAL_ECHO(backlash.get_measurement(a));
}
}
- if (!backlash_measured_num[X_AXIS] && !backlash_measured_num[Y_AXIS] && !backlash_measured_num[Z_AXIS])
+ else
SERIAL_ECHOPGM(" (Not yet measured)");
SERIAL_EOL();
#endif
diff --git a/marlin-original-clean/Marlin/src/gcode/feature/runout/M412.cpp b/marlin-qq-clean/Marlin/src/gcode/feature/runout/M412.cpp
index 749ce2a..6cf2238 100644
--- a/marlin-original-clean/Marlin/src/gcode/feature/runout/M412.cpp
+++ b/marlin-qq-clean/Marlin/src/gcode/feature/runout/M412.cpp
@@ -32,6 +32,9 @@
*/
void GcodeSuite::M412() {
if (parser.seen("HS"
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ "D"
+ #endif
#if ENABLED(HOST_ACTION_COMMANDS)
"R"
#endif
@@ -42,11 +45,17 @@ void GcodeSuite::M412() {
const bool seenR = parser.seen('R'), seenS = parser.seen('S');
if (seenR || seenS) runout.reset();
if (seenS) runout.enabled = parser.value_bool();
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ if (parser.seen('D')) runout.set_runout_distance(parser.value_linear_units());
+ #endif
}
else {
SERIAL_ECHO_START();
SERIAL_ECHOPGM("Filament runout ");
serialprintln_onoff(runout.enabled);
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ SERIAL_ECHOLNPAIR("Filament runout distance (mm): ", runout.runout_distance());
+ #endif
}
}
diff --git a/marlin-original-clean/Marlin/src/inc/Conditionals_LCD.h b/marlin-qq-clean/Marlin/src/inc/Conditionals_LCD.h
index 45051e3..898790d 100644
--- a/marlin-original-clean/Marlin/src/inc/Conditionals_LCD.h
+++ b/marlin-qq-clean/Marlin/src/inc/Conditionals_LCD.h
@@ -237,6 +237,8 @@
#define ULTRA_LCD
#define DOGLCD
#define ULTIPANEL
+ #define RGB_DISPLAY
+ #define TOUCH_BUTTONS
#endif
/**
@@ -516,9 +518,13 @@
#define GRID_MAX_POINTS ((GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y))
#endif
+#if ENABLED(MALYAN_LCD)
+ #define EXTENSIBLE_UI
+#endif
#define HAS_SOFTWARE_ENDSTOPS EITHER(MIN_SOFTWARE_ENDSTOPS, MAX_SOFTWARE_ENDSTOPS)
#define HAS_RESUME_CONTINUE ANY(EXTENSIBLE_UI, NEWPANEL, EMERGENCY_PARSER)
-#define HAS_COLOR_LEDS ANY(BLINKM, RGB_LED, RGBW_LED, PCA9632, PCA9533, NEOPIXEL_LED)
+//#define HAS_COLOR_LEDS ANY(BLINKM, RGB_LED, RGBW_LED, PCA9632, PCA9533, NEOPIXEL_LED)
+#define HAS_COLOR_LEDS (ENABLED(BLINKM) || ENABLED(RGB_LED) || ENABLED(RGBW_LED) || ENABLED(PCA9532) || ENABLED(NEOPIXEL_LED) || ENABLED (RGB_DISPLAY))
#define HAS_LEDS_OFF_FLAG (BOTH(PRINTER_EVENT_LEDS, SDSUPPORT) && HAS_RESUME_CONTINUE)
#define HAS_PRINT_PROGRESS EITHER(SDSUPPORT, LCD_SET_PROGRESS_MANUALLY)
#define HAS_SERVICE_INTERVALS (SERVICE_INTERVAL_1 > 0 || SERVICE_INTERVAL_2 > 0 || SERVICE_INTERVAL_3 > 0)
diff --git a/marlin-original-clean/Marlin/src/inc/Conditionals_post.h b/marlin-qq-clean/Marlin/src/inc/Conditionals_post.h
index a4789f7..3db857e 100644
--- a/marlin-original-clean/Marlin/src/inc/Conditionals_post.h
+++ b/marlin-qq-clean/Marlin/src/inc/Conditionals_post.h
@@ -53,7 +53,8 @@
#undef FILAMENT_LOAD_UNLOAD_GCODES
#endif
-#define HAS_CLASSIC_JERK (IS_KINEMATIC || DISABLED(JUNCTION_DEVIATION))
+// TODO
+//#define HAS_CLASSIC_JERK (IS_KINEMATIC || DISABLED(JUNCTION_DEVIATION))
/**
* Axis lengths and center
diff --git a/marlin-original-clean/Marlin/src/inc/SanityCheck.h b/marlin-qq-clean/Marlin/src/inc/SanityCheck.h
index a268595..3f80764 100644
--- a/marlin-original-clean/Marlin/src/inc/SanityCheck.h
+++ b/marlin-qq-clean/Marlin/src/inc/SanityCheck.h
@@ -613,6 +613,8 @@ static_assert(Y_MAX_LENGTH >= Y_BED_SIZE, "Movement bounds (Y_MIN_POS, Y_MAX_POS
#error "FILAMENT_RUNOUT_SENSOR with NUM_RUNOUT_SENSORS > 5 requires FIL_RUNOUT6_PIN."
#elif DISABLED(SDSUPPORT, PRINTJOB_TIMER_AUTOSTART)
#error "FILAMENT_RUNOUT_SENSOR requires SDSUPPORT or PRINTJOB_TIMER_AUTOSTART."
+ #elif FILAMENT_RUNOUT_DISTANCE_MM < 0
+ #error "FILAMENT_RUNOUT_DISTANCE_MM must be greater than or equal to zero."
#elif DISABLED(ADVANCED_PAUSE_FEATURE)
static_assert(NULL == strstr(FILAMENT_RUNOUT_SCRIPT, "M600"), "ADVANCED_PAUSE_FEATURE is required to use M600 with FILAMENT_RUNOUT_SENSOR.");
#endif
@@ -1784,7 +1786,7 @@ static_assert(Y_MAX_LENGTH >= Y_BED_SIZE, "Movement bounds (Y_MIN_POS, Y_MAX_POS
+ ENABLED(OLED_PANEL_TINYBOY2) \
+ ENABLED(ZONESTAR_LCD) \
+ ENABLED(ULTI_CONTROLLER) \
- + ENABLED(EXTENSIBLE_UI)
+ + (ENABLED(EXTENSIBLE_UI) && DISABLED(MALYAN_LCD))
#error "Please select no more than one LCD controller option."
#endif
diff --git a/marlin-original-clean/Marlin/src/lcd/dogm/u8g_dev_tft_320x240_upscale_from_128x64.cpp b/marlin-qq-clean/Marlin/src/lcd/dogm/u8g_dev_tft_320x240_upscale_from_128x64.cpp
index 8f295ab..7904b2b 100644
--- a/marlin-original-clean/Marlin/src/lcd/dogm/u8g_dev_tft_320x240_upscale_from_128x64.cpp
+++ b/marlin-qq-clean/Marlin/src/lcd/dogm/u8g_dev_tft_320x240_upscale_from_128x64.cpp
@@ -1,6 +1,6 @@
/**
* Marlin 3D Printer Firmware
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
@@ -63,109 +63,361 @@
#include "HAL_LCD_com_defines.h"
#include "string.h"
+#include "../../lcd/ultralcd.h"
+#if HAS_COLOR_LEDS && ENABLED(PRINTER_EVENT_LEDS)
+#include "../../feature/leds/leds.h"
+#endif
+
+struct LCD_IO {
+ uint32_t id;
+ void (*writeRegister)(uint16_t reg);
+ uint16_t (*readData)(void);
+ void (*writeData)(uint16_t data);
+ void (*writeMultiple)(uint16_t data, uint32_t count);
+ void (*writeSequence)(uint16_t *data, uint16_t length);
+ void (*setWindow)(uint16_t Xmin, uint16_t Ymin, uint16_t Xmax, uint16_t Ymax);
+};
+static LCD_IO lcd = {0, NULL, NULL, NULL, NULL, NULL, NULL};
+
#define WIDTH 128
#define HEIGHT 64
#define PAGE_HEIGHT 8
#define X_MIN 32
-#define Y_MIN 56
+#define Y_MIN 28
#define X_MAX (X_MIN + 2 * WIDTH - 1)
#define Y_MAX (Y_MIN + 2 * HEIGHT - 1)
-#define LCD_COLUMN 0x2A /* Colomn address register */
-#define LCD_ROW 0x2B /* Row address register */
-#define LCD_WRITE_RAM 0x2C
-
static uint32_t lcd_id = 0;
+uint16_t color = 0xFFFF;
+
+#define ESC_REG(x) 0xFFFF, 0x00FF & (uint16_t)x
+#define ESC_DELAY(x) 0xFFFF, 0x8000 | (x & 0x7FFF)
+#define ESC_END 0xFFFF, 0x7FFF
+#define ESC_FFFF 0xFFFF, 0xFFFF
-#define U8G_ESC_DATA(x) (uint8_t)(x >> 8), (uint8_t)(x & 0xFF)
+void writeEscSequence(const uint16_t *sequence) {
+ uint16_t data;
+ for (;;) {
+ data = *sequence++;
+ if (data != 0xFFFF) {
+ lcd.writeData(data);
+ continue;
+ }
+ data = *sequence++;
+ if (data == 0x7FFF) return;
+ if (data == 0xFFFF) {
+ lcd.writeData(data);
+ } else if (data & 0x8000) {
+ delay(data & 0x7FFF);
+ } else if ((data & 0xFF00) == 0) {
+ lcd.writeRegister(data);
+ }
+ }
+}
-static const uint8_t page_first_sequence[] = {
- U8G_ESC_ADR(0), LCD_COLUMN, U8G_ESC_ADR(1), U8G_ESC_DATA(X_MIN), U8G_ESC_DATA(X_MAX),
- U8G_ESC_ADR(0), LCD_ROW, U8G_ESC_ADR(1), U8G_ESC_DATA(Y_MIN), U8G_ESC_DATA(Y_MAX),
- U8G_ESC_ADR(0), LCD_WRITE_RAM, U8G_ESC_ADR(1),
- U8G_ESC_END
+static const uint16_t st7789v_init[] = {
+ ESC_REG(0x10), ESC_DELAY(10), ESC_REG(0x01), ESC_DELAY(200), ESC_REG(0x11), ESC_DELAY(120),
+ ESC_REG(0x36), 0x00A0,
+ ESC_REG(0x3A), 0x0055,
+ ESC_REG(0x2A), 0x0000, 0x0000, 0x0001, 0x003F,
+ ESC_REG(0x2B), 0x0000, 0x0000, 0x0000, 0x00EF,
+ ESC_REG(0xB2), 0x000C, 0x000C, 0x0000, 0x0033, 0x0033,
+ ESC_REG(0xB7), 0x0035,
+ ESC_REG(0xBB), 0x001F,
+ ESC_REG(0xC0), 0x002C,
+ ESC_REG(0xC2), 0x0001, 0x00C3,
+ ESC_REG(0xC4), 0x0020,
+ ESC_REG(0xC6), 0x000F,
+ ESC_REG(0xD0), 0x00A4, 0x00A1,
+ ESC_REG(0x29),
+ ESC_REG(0x11),
+ ESC_END
};
-static const uint8_t clear_screen_sequence[] = {
- U8G_ESC_ADR(0), LCD_COLUMN, U8G_ESC_ADR(1), 0x00, 0x00, 0x01, 0x3F,
- U8G_ESC_ADR(0), LCD_ROW, U8G_ESC_ADR(1), 0x00, 0x00, 0x00, 0xEF,
- U8G_ESC_ADR(0), LCD_WRITE_RAM, U8G_ESC_ADR(1),
- U8G_ESC_END
+static const uint16_t ili9328_init[] = {
+ ESC_REG(0x0001), 0x0100,
+ ESC_REG(0x0002), 0x0400,
+ ESC_REG(0x0003), 0x1038,
+ ESC_REG(0x0004), 0x0000,
+ ESC_REG(0x0008), 0x0202,
+ ESC_REG(0x0009), 0x0000,
+ ESC_REG(0x000A), 0x0000,
+ ESC_REG(0x000C), 0x0000,
+ ESC_REG(0x000D), 0x0000,
+ ESC_REG(0x000F), 0x0000,
+ ESC_REG(0x0010), 0x0000,
+ ESC_REG(0x0011), 0x0007,
+ ESC_REG(0x0012), 0x0000,
+ ESC_REG(0x0013), 0x0000,
+ ESC_REG(0x0007), 0x0001,
+ ESC_DELAY(200),
+ ESC_REG(0x0010), 0x1690,
+ ESC_REG(0x0011), 0x0227,
+ ESC_DELAY(50),
+ ESC_REG(0x0012), 0x008C,
+ ESC_DELAY(50),
+ ESC_REG(0x0013), 0x1500,
+ ESC_REG(0x0029), 0x0004,
+ ESC_REG(0x002B), 0x000D,
+ ESC_DELAY(50),
+ ESC_REG(0x0050), 0x0000,
+ ESC_REG(0x0051), 0x00EF,
+ ESC_REG(0x0052), 0x0000,
+ ESC_REG(0x0053), 0x013F,
+ ESC_REG(0x0020), 0x0000,
+ ESC_REG(0x0021), 0x0000,
+ ESC_REG(0x0060), 0x2700,
+ ESC_REG(0x0061), 0x0001,
+ ESC_REG(0x006A), 0x0000,
+ ESC_REG(0x0080), 0x0000,
+ ESC_REG(0x0081), 0x0000,
+ ESC_REG(0x0082), 0x0000,
+ ESC_REG(0x0083), 0x0000,
+ ESC_REG(0x0084), 0x0000,
+ ESC_REG(0x0085), 0x0000,
+ ESC_REG(0x0090), 0x0010,
+ ESC_REG(0x0092), 0x0600,
+ ESC_REG(0x0007), 0x0133,
+ ESC_REG(0x0022),
+ ESC_END
};
-static const uint8_t st7789v_init_sequence[] = { // 0x8552 - ST7789V
- U8G_ESC_ADR(0),
- 0x10,
- U8G_ESC_DLY(10),
- 0x01,
- U8G_ESC_DLY(100), U8G_ESC_DLY(100),
- 0x11,
- U8G_ESC_DLY(120),
- 0x36, U8G_ESC_ADR(1), 0xA0,
- U8G_ESC_ADR(0), 0x3A, U8G_ESC_ADR(1), 0x05,
- U8G_ESC_ADR(0), LCD_COLUMN, U8G_ESC_ADR(1), 0x00, 0x00, 0x01, 0x3F,
- U8G_ESC_ADR(0), LCD_ROW, U8G_ESC_ADR(1), 0x00, 0x00, 0x00, 0xEF,
- U8G_ESC_ADR(0), 0xB2, U8G_ESC_ADR(1), 0x0C, 0x0C, 0x00, 0x33, 0x33,
- U8G_ESC_ADR(0), 0xB7, U8G_ESC_ADR(1), 0x35,
- U8G_ESC_ADR(0), 0xBB, U8G_ESC_ADR(1), 0x1F,
- U8G_ESC_ADR(0), 0xC0, U8G_ESC_ADR(1), 0x2C,
- U8G_ESC_ADR(0), 0xC2, U8G_ESC_ADR(1), 0x01, 0xC3,
- U8G_ESC_ADR(0), 0xC4, U8G_ESC_ADR(1), 0x20,
- U8G_ESC_ADR(0), 0xC6, U8G_ESC_ADR(1), 0x0F,
- U8G_ESC_ADR(0), 0xD0, U8G_ESC_ADR(1), 0xA4, 0xA1,
- U8G_ESC_ADR(0), 0xE0, U8G_ESC_ADR(1), 0xD0, 0x08, 0x11, 0x08, 0x0C, 0x15, 0x39, 0x33, 0x50, 0x36, 0x13, 0x14, 0x29, 0x2D,
- U8G_ESC_ADR(0), 0xE1, U8G_ESC_ADR(1), 0xD0, 0x08, 0x10, 0x08, 0x06, 0x06, 0x39, 0x44, 0x51, 0x0B, 0x16, 0x14, 0x2F, 0x31,
- U8G_ESC_ADR(0), 0x29, 0x11, 0x35, U8G_ESC_ADR(1), 0x00,
- U8G_ESC_END
+static const uint8_t button0[] = {
+ B01111111,B11111111,B11111111,B11111111,B11111110,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00001000,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00111110,B00000000,B00000001,
+ B10000000,B00000000,B01111111,B00000000,B00000001,
+ B10000000,B00000000,B11111111,B10000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B01111111,B11111111,B11111111,B11111111,B11111110,
};
+static const uint8_t button1[] = {
+ B01111111,B11111111,B11111111,B11111111,B11111110,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B11111111,B10000000,B00000001,
+ B10000000,B00000000,B01111111,B00000000,B00000001,
+ B10000000,B00000000,B00111110,B00000000,B00000001,
+ B10000000,B00000000,B00011100,B00000000,B00000001,
+ B10000000,B00000000,B00001000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B01111111,B11111111,B11111111,B11111111,B11111110,
+};
+
+static const uint8_t button2[] = {
+ B01111111,B11111111,B11111111,B11111111,B11111110,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B01000001,B11000000,B00000001,
+ B10000000,B00000000,B11000001,B11000000,B00000001,
+ B10000000,B00000001,B11111111,B11000000,B00000001,
+ B10000000,B00000011,B11111111,B11000000,B00000001,
+ B10000000,B00000001,B11111111,B11000000,B00000001,
+ B10000000,B00000000,B11000000,B00000000,B00000001,
+ B10000000,B00000000,B01000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B10000000,B00000000,B00000000,B00000000,B00000001,
+ B01111111,B11111111,B11111111,B11111111,B11111110,
+};
+
+static void _setWindow_x20_x21_x22(uint16_t Xmin, uint16_t Ymin, uint16_t Xmax, uint16_t Ymax) {
+ lcd.writeRegister(0x50);
+ lcd.writeData(Ymin);
+ lcd.writeRegister(0x51);
+ lcd.writeData(Ymax);
+ lcd.writeRegister(0x52);
+ lcd.writeData(Xmin);
+ lcd.writeRegister(0x53);
+ lcd.writeData(Xmax);
+
+ lcd.writeRegister(0x20);
+ lcd.writeData(Ymin);
+ lcd.writeRegister(0x21);
+ lcd.writeData(Xmin);
+
+ lcd.writeRegister(0x22);
+}
+
+static void _setWindow_x2a_x2b_x2c(uint16_t Xmin, uint16_t Ymin, uint16_t Xmax, uint16_t Ymax) {
+ lcd.writeRegister(0x2A);
+ lcd.writeData((Xmin >> 8) & 0xFF);
+ lcd.writeData(Xmin & 0xFF);
+ lcd.writeData((Xmax >> 8) & 0xFF);
+ lcd.writeData(Xmax & 0xFF);
+
+ lcd.writeRegister(0x2B);
+ lcd.writeData((Ymin >> 8) & 0xFF);
+ lcd.writeData(Ymin & 0xFF);
+ lcd.writeData((Ymax >> 8) & 0xFF);
+ lcd.writeData(Ymax & 0xFF);
+
+ lcd.writeRegister(0x2C);
+}
+
+void drawImage(const uint8_t *data, uint16_t length, uint16_t height) {
+ uint16_t i, j, k;
+ uint16_t buffer[160];
+
+ for (i = 0; i < height; i++) {
+ k = 0;
+ for (j = 0; j < length; j++) {
+ if (*(data + (i * (length >> 3) + (j >> 3))) & (128 >> (j & 7))) {
+ buffer[k++] = color;
+ buffer[k++] = color;
+ } else {
+ buffer[k++] = 0x0000;
+ buffer[k++] = 0x0000;
+ }
+ }
+ lcd.writeSequence(buffer, length << 1);
+ lcd.writeSequence(buffer, length << 1);
+ }
+}
+
+void drawUI(void) {
+ lcd.setWindow(10, 170, 309, 171);
+ lcd.writeMultiple(color, 600);
+
+ lcd.setWindow( 20, 185, 99, 224);
+ drawImage(button0, 40, 20);
+ lcd.setWindow(120, 185, 199, 224);
+ drawImage(button1, 40, 20);
+ lcd.setWindow(220, 185, 299, 224);
+ drawImage(button2, 40, 20);
+}
+
uint8_t u8g_dev_tft_320x240_upscale_from_128x64_fn(u8g_t *u8g, u8g_dev_t *dev, uint8_t msg, void *arg) {
+#if HAS_COLOR_LEDS && ENABLED(PRINTER_EVENT_LEDS)
+ uint16_t newColor;
+#endif
u8g_pb_t *pb = (u8g_pb_t *)(dev->dev_mem);
uint16_t buffer[256];
uint32_t i, j, k;
+ uint8_t byte;
- switch (msg) {
+ uint16_t reg00;
+
+ switch(msg) {
case U8G_DEV_MSG_INIT:
- dev->com_fn(u8g, U8G_COM_MSG_INIT, U8G_SPI_CLK_CYCLE_NONE, &lcd_id);
- if (lcd_id == 0x040404) return 0; // No connected display on FSMC
- if (lcd_id == 0xFFFFFF) return 0; // No connected display on SPI
+ dev->com_fn(u8g, U8G_COM_MSG_INIT, U8G_SPI_CLK_CYCLE_NONE, &lcd);
+ if (lcd.writeRegister == NULL || lcd.writeData == NULL || lcd.readData == NULL || lcd.writeSequence == NULL) break;
- memset(buffer, 0x00, sizeof(buffer));
+// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+// !!! POC implementation. NOT compatible with 8-bit interface (SPI / FSMC 8-bit) !!!
+// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
- if ((lcd_id & 0xFFFF) == 0x8552) // ST7789V
- u8g_WriteEscSeqP(u8g, dev, st7789v_init_sequence);
+ lcd.writeRegister(0x0000);
+ reg00 = lcd.readData();
+ if (reg00 == 0 || reg00 == 0xFFFF) {
+ lcd.writeRegister(0x0004);
+ lcd.readData(); // dummy read
+ lcd.id = ((uint32_t)(lcd.readData() & 0xff) << 16) | ((uint32_t)(lcd.readData() & 0xff) << 8) | (uint32_t)(lcd.readData() & 0xff);
+ if (lcd_id == 0x040404) { // No connected display on FSMC
+ lcd.id = 0;
+ return 0;
+ }
+ } else {
+ lcd.id = (uint32_t)reg00;
+ }
+
+ switch(lcd.id & 0xFFFF) {
+ case 0x8552: // ST7789V
+ writeEscSequence(st7789v_init);
+ lcd.setWindow = _setWindow_x2a_x2b_x2c;
+ break;
+ case 0x9328: // ILI9328
+ writeEscSequence(ili9328_init);
+ lcd.setWindow = _setWindow_x20_x21_x22;
+ break;
+ case 0x0404: // No connected display on FSMC
+ lcd.id = 0;
+ return 0;
+ case 0xFFFF: // No connected display on SPI
+ lcd.id = 0;
+ return 0;
+ default:
+ if (reg00 == 0)
+ lcd.setWindow = _setWindow_x2a_x2b_x2c;
+ else
+ lcd.setWindow = _setWindow_x20_x21_x22;
+ break;
+ }
- u8g_WriteEscSeqP(u8g, dev, clear_screen_sequence);
- for (i = 0; i < 960; i++)
- u8g_WriteSequence(u8g, dev, 160, (uint8_t *)buffer);
+ lcd.setWindow(0,0,319,239);
+ lcd.writeMultiple(0x0000, 320 * 240);
+ drawUI();
break;
case U8G_DEV_MSG_STOP:
break;
case U8G_DEV_MSG_PAGE_FIRST:
- u8g_WriteEscSeqP(u8g, dev, page_first_sequence);
+ if (lcd.id == 0) break;
+
+#if HAS_COLOR_LEDS && ENABLED(PRINTER_EVENT_LEDS)
+ newColor = (0xF800 & (((uint16_t)leds.color.r) << 8)) | (0x07E0 & (((uint16_t)leds.color.g) << 3)) | (0x001F & (((uint16_t)leds.color.b) >> 3));
+ if ((newColor != 0) && (newColor != color)) {
+ color = newColor;
+ drawUI();
+ }
+#endif
+ lcd.setWindow(X_MIN,Y_MIN,X_MAX,Y_MAX);
break;
case U8G_DEV_MSG_PAGE_NEXT:
- for (j = 0; j < 8; j++) {
+ if (lcd.id == 0) break;
+
+ for (j = 0; j < 8; j++) {
k = 0;
- for (i = 0; i < (uint32_t)pb->width; i++) {
- const uint8_t b = *(((uint8_t *)pb->buf) + i);
- const uint16_t c = TEST(b, j) ? 0x7FFF : 0x0000;
- buffer[k++] = c; buffer[k++] = c;
- }
- for (k = 0; k < 2; k++) {
- u8g_WriteSequence(u8g, dev, 128, (uint8_t*)buffer);
- u8g_WriteSequence(u8g, dev, 128, (uint8_t*)&(buffer[64]));
- u8g_WriteSequence(u8g, dev, 128, (uint8_t*)&(buffer[128]));
- u8g_WriteSequence(u8g, dev, 128, (uint8_t*)&(buffer[192]));
+ for (i = 0; i < (uint32_t) pb->width; i++) {
+ byte = *(((uint8_t *)pb->buf) + i);
+ if (byte & (1 << j)) {
+ buffer[k++] = color;
+ buffer[k++] = color;
+ } else {
+ buffer[k++] = 0x0000;
+ buffer[k++] = 0x0000;
+ }
}
+ for (k = 0; k < 2; k++) lcd.writeSequence(buffer, 256);
}
break;
case U8G_DEV_MSG_SLEEP_ON:
+ return 1;
+
case U8G_DEV_MSG_SLEEP_OFF:
return 1;
}
diff --git a/marlin-original-clean/Marlin/src/lcd/extensible_ui/lib/example.cpp b/marlin-qq-clean/Marlin/src/lcd/extensible_ui/lib/example.cpp
index 2a11d5c..5a52e26 100644
--- a/marlin-original-clean/Marlin/src/lcd/extensible_ui/lib/example.cpp
+++ b/marlin-qq-clean/Marlin/src/lcd/extensible_ui/lib/example.cpp
@@ -1,6 +1,6 @@
-/*************
- * dummy.cpp *
- *************/
+/***************
+ * example.cpp *
+ ***************/
/****************************************************************************
* Written By Marcio Teixeira 2018 - Aleph Objects, Inc. *
@@ -21,7 +21,7 @@
#include "../../../inc/MarlinConfigPre.h"
-#if ENABLED(EXTENSIBLE_UI)
+#if BOTH(EXTUI_EXAMPLE, EXTENSIBLE_UI)
#include "../ui_api.h"
@@ -58,8 +58,36 @@ namespace ExtUI {
void onUserConfirmRequired(const char * const msg) {}
void onStatusChanged(const char * const msg) {}
void onFactoryReset() {}
- void onLoadSettings() {}
- void onStoreSettings() {}
+
+ void onStoreSettings(char *buff) {
+ // This is called when saving to EEPROM (i.e. M500). If the ExtUI needs
+ // permanent data to be stored, it can write up to eeprom_data_size bytes
+ // into buff.
+
+ // Example:
+ // static_assert(sizeof(myDataStruct) <= ExtUI::eeprom_data_size);
+ // memcpy(buff, &myDataStruct, sizeof(myDataStruct));
+ }
+
+ void onLoadSettings(const char *buff) {
+ // This is called while loading settings from EEPROM. If the ExtUI
+ // needs to retrieve data, it should copy up to eeprom_data_size bytes
+ // from buff
+
+ // Example:
+ // static_assert(sizeof(myDataStruct) <= ExtUI::eeprom_data_size);
+ // memcpy(&myDataStruct, buff, sizeof(myDataStruct));
+ }
+
+ void onConfigurationStoreWritten(bool success) {
+ // This is called after the entire EEPROM has been written,
+ // whether successful or not.
+ }
+
+ void onConfigurationStoreRead(bool success) {
+ // This is called after the entire EEPROM has been read,
+ // whether successful or not.
+ }
}
-#endif // EXTENSIBLE_UI
+#endif // EXTUI_EXAMPLE && EXTENSIBLE_UI
diff --git a/marlin-original-clean/Marlin/src/lcd/extensible_ui/ui_api.cpp b/marlin-qq-clean/Marlin/src/lcd/extensible_ui/ui_api.cpp
index 8e531ec..a140896 100644
--- a/marlin-original-clean/Marlin/src/lcd/extensible_ui/ui_api.cpp
+++ b/marlin-qq-clean/Marlin/src/lcd/extensible_ui/ui_api.cpp
@@ -82,11 +82,7 @@
#include "ui_api.h"
#if ENABLED(BACKLASH_GCODE)
- extern float backlash_distance_mm[XYZ];
- extern uint8_t backlash_correction;
- #ifdef BACKLASH_SMOOTHING_MM
- extern float backlash_smoothing_mm;
- #endif
+ #include "../../feature/backlash.h"
#endif
#if HAS_LEVELING
@@ -111,7 +107,6 @@ static struct {
} flags;
namespace ExtUI {
-
#ifdef __SAM3X8E__
/**
* Implement a special millis() to allow time measurement
@@ -517,13 +512,13 @@ namespace ExtUI {
bool getFilamentRunoutEnabled() { return runout.enabled; }
void setFilamentRunoutEnabled(const bool value) { runout.enabled = value; }
- #if FILAMENT_RUNOUT_DISTANCE_MM > 0
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
float getFilamentRunoutDistance_mm() {
- return RunoutResponseDelayed::runout_distance_mm;
+ return runout.runout_distance();
}
void setFilamentRunoutDistance_mm(const float value) {
- RunoutResponseDelayed::runout_distance_mm = clamp(value, 0, 999);
+ runout.set_runout_distance(clamp(value, 0, 999));
}
#endif
#endif
@@ -687,16 +682,16 @@ namespace ExtUI {
#endif // HAS_HOTEND_OFFSET
#if ENABLED(BACKLASH_GCODE)
- float getAxisBacklash_mm(const axis_t axis) { return backlash_distance_mm[axis]; }
+ float getAxisBacklash_mm(const axis_t axis) { return backlash.distance_mm[axis]; }
void setAxisBacklash_mm(const float value, const axis_t axis)
- { backlash_distance_mm[axis] = clamp(value,0,5); }
+ { backlash.distance_mm[axis] = clamp(value,0,5); }
- float getBacklashCorrection_percent() { return ui8_to_percent(backlash_correction); }
- void setBacklashCorrection_percent(const float value) { backlash_correction = map(clamp(value, 0, 100), 0, 100, 0, 255); }
+ float getBacklashCorrection_percent() { return ui8_to_percent(backlash.correction); }
+ void setBacklashCorrection_percent(const float value) { backlash.correction = map(clamp(value, 0, 100), 0, 100, 0, 255); }
#ifdef BACKLASH_SMOOTHING_MM
- float getBacklashSmoothing_mm() { return backlash_smoothing_mm; }
- void setBacklashSmoothing_mm(const float value) { backlash_smoothing_mm = clamp(value, 0, 999); }
+ float getBacklashSmoothing_mm() { return backlash.smoothing_mm; }
+ void setBacklashSmoothing_mm(const float value) { backlash.smoothing_mm = clamp(value, 0, 999); }
#endif
#endif
@@ -750,7 +745,7 @@ namespace ExtUI {
}
bool commandsInQueue() { return (planner.movesplanned() || commands_in_queue); }
-
+
bool isAxisPositionKnown(const axis_t axis) {
return TEST(axis_known_position, axis);
}
diff --git a/marlin-original-clean/Marlin/src/lcd/extensible_ui/ui_api.h b/marlin-qq-clean/Marlin/src/lcd/extensible_ui/ui_api.h
index 2f2f99d..a643ffd 100644
--- a/marlin-original-clean/Marlin/src/lcd/extensible_ui/ui_api.h
+++ b/marlin-qq-clean/Marlin/src/lcd/extensible_ui/ui_api.h
@@ -46,6 +46,11 @@
#include "../../inc/MarlinConfig.h"
namespace ExtUI {
+ // The ExtUI implementation can store up to this many bytes
+ // in the EEPROM when the methods onStoreSettings and
+ // onLoadSettings are called.
+
+ static constexpr size_t eeprom_data_size = 48;
enum axis_t : uint8_t { X, Y, Z };
enum extruder_t : uint8_t { E0, E1, E2, E3, E4, E5 };
@@ -207,7 +212,7 @@ namespace ExtUI {
bool getFilamentRunoutEnabled();
void setFilamentRunoutEnabled(const bool);
- #if FILAMENT_RUNOUT_DISTANCE_MM > 0
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
float getFilamentRunoutDistance_mm();
void setFilamentRunoutDistance_mm(const float);
#endif
@@ -283,8 +288,10 @@ namespace ExtUI {
void onUserConfirmRequired(const char * const msg);
void onStatusChanged(const char * const msg);
void onFactoryReset();
- void onStoreSettings();
- void onLoadSettings();
+ void onStoreSettings(char *);
+ void onLoadSettings(const char *);
+ void onConfigurationStoreWritten(bool success);
+ void onConfigurationStoreRead(bool success);
};
/**
diff --git a/marlin-original-clean/Marlin/src/lcd/malyanlcd.cpp b/marlin-qq-clean/Marlin/src/lcd/malyanlcd.cpp
index cb0e2a0..8a6d611 100644
--- a/marlin-original-clean/Marlin/src/lcd/malyanlcd.cpp
+++ b/marlin-qq-clean/Marlin/src/lcd/malyanlcd.cpp
@@ -41,23 +41,19 @@
* Copyright (c) 2017 Jason Nelson (xC0000005)
*/
-#include "../inc/MarlinConfig.h"
+#include "../inc/MarlinConfigPre.h"
#if ENABLED(MALYAN_LCD)
+#include "extensible_ui/ui_api.h"
+
#include "ultralcd.h"
#include "../module/temperature.h"
-#include "../module/planner.h"
#include "../module/stepper.h"
#include "../module/motion.h"
-#include "../module/probe.h"
#include "../libs/duration_t.h"
#include "../module/printcounter.h"
-#include "../gcode/gcode.h"
#include "../gcode/queue.h"
-#include "../module/configuration_store.h"
-
-#include "../Marlin.h"
#if ENABLED(SDSUPPORT)
#include "../sd/cardreader.h"
@@ -412,78 +408,118 @@ void update_usb_status(const bool forceUpdate) {
}
}
-/**
- * - from printer on startup:
- * {SYS:STARTED}{VER:29}{SYS:STARTED}{R:UD}
- * The optimize attribute fixes a register Compile
- * error for amtel.
- */
-void MarlinUI::update() {
- static char inbound_buffer[MAX_CURLY_COMMAND];
-
- // First report USB status.
- update_usb_status(false);
-
- // now drain commands...
- while (LCD_SERIAL.available()) {
- const byte b = (byte)LCD_SERIAL.read() & 0x7F;
- inbound_buffer[inbound_count++] = b;
- if (b == '}' || inbound_count == sizeof(inbound_buffer) - 1) {
- inbound_buffer[inbound_count - 1] = '\0';
- process_lcd_command(inbound_buffer);
- inbound_count = 0;
- inbound_buffer[0] = 0;
- }
+namespace ExtUI {
+ void onStartup() {
+ /**
+ * The Malyan LCD actually runs as a separate MCU on Serial 1.
+ * This code's job is to siphon the weird curly-brace commands from
+ * it and translate into gcode, which then gets injected into
+ * the command queue where possible.
+ */
+ inbound_count = 0;
+ LCD_SERIAL.begin(500000);
+
+ // Signal init
+ write_to_lcd_P(PSTR("{SYS:STARTED}\r\n"));
+
+ // send a version that says "unsupported"
+ write_to_lcd_P(PSTR("{VER:99}\r\n"));
+
+ // No idea why it does this twice.
+ write_to_lcd_P(PSTR("{SYS:STARTED}\r\n"));
+ update_usb_status(true);
}
- #if ENABLED(SDSUPPORT)
- // The way last printing status works is simple:
- // The UI needs to see at least one TQ which is not 100%
- // and then when the print is complete, one which is.
- static uint8_t last_percent_done = 100;
-
- // If there was a print in progress, we need to emit the final
- // print status as {TQ:100}. Reset last percent done so a new print will
- // issue a percent of 0.
- const uint8_t percent_done = IS_SD_PRINTING() ? card.percentDone() : last_printing_status ? 100 : 0;
- if (percent_done != last_percent_done) {
- char message_buffer[10];
- sprintf_P(message_buffer, PSTR("{TQ:%03i}"), percent_done);
- write_to_lcd(message_buffer);
- last_percent_done = percent_done;
- last_printing_status = IS_SD_PRINTING();
+ void onIdle() {
+ /**
+ * - from printer on startup:
+ * {SYS:STARTED}{VER:29}{SYS:STARTED}{R:UD}
+ * The optimize attribute fixes a register Compile
+ * error for amtel.
+ */
+ static char inbound_buffer[MAX_CURLY_COMMAND];
+
+ // First report USB status.
+ update_usb_status(false);
+
+ // now drain commands...
+ while (LCD_SERIAL.available()) {
+ const byte b = (byte)LCD_SERIAL.read() & 0x7F;
+ inbound_buffer[inbound_count++] = b;
+ if (b == '}' || inbound_count == sizeof(inbound_buffer) - 1) {
+ inbound_buffer[inbound_count - 1] = '\0';
+ process_lcd_command(inbound_buffer);
+ inbound_count = 0;
+ inbound_buffer[0] = 0;
+ }
}
- #endif
-}
-/**
- * The Malyan LCD actually runs as a separate MCU on Serial 1.
- * This code's job is to siphon the weird curly-brace commands from
- * it and translate into gcode, which then gets injected into
- * the command queue where possible.
- */
-void MarlinUI::init() {
- inbound_count = 0;
- LCD_SERIAL.begin(500000);
+ #if ENABLED(SDSUPPORT)
+ // The way last printing status works is simple:
+ // The UI needs to see at least one TQ which is not 100%
+ // and then when the print is complete, one which is.
+ static uint8_t last_percent_done = 100;
+
+ // If there was a print in progress, we need to emit the final
+ // print status as {TQ:100}. Reset last percent done so a new print will
+ // issue a percent of 0.
+ const uint8_t percent_done = IS_SD_PRINTING() ? card.percentDone() : last_printing_status ? 100 : 0;
+ if (percent_done != last_percent_done) {
+ char message_buffer[10];
+ sprintf_P(message_buffer, PSTR("{TQ:%03i}"), percent_done);
+ write_to_lcd(message_buffer);
+ last_percent_done = percent_done;
+ last_printing_status = IS_SD_PRINTING();
+ }
+ #endif
+ }
- // Signal init
- write_to_lcd_P(PSTR("{SYS:STARTED}\r\n"));
+ void onPrinterKilled(PGM_P const msg) {}
+ void onMediaInserted() {};
+ void onMediaError() {};
+ void onMediaRemoved() {};
+ void onPlayTone(const uint16_t frequency, const uint16_t duration) {}
+ void onPrintTimerStarted() {}
+ void onPrintTimerPaused() {}
+ void onPrintTimerStopped() {}
+ void onFilamentRunout() {}
+ void onUserConfirmRequired(const char * const msg) {}
+ void onStatusChanged(const char * const msg) {
+ write_to_lcd_P(PSTR("{E:"));
+ write_to_lcd(msg);
+ write_to_lcd_P("}");
+ }
+ void onFactoryReset() {}
- // send a version that says "unsupported"
- write_to_lcd_P(PSTR("{VER:99}\r\n"));
+ void onStoreSettings(char *buff) {
+ // This is called when saving to EEPROM (i.e. M500). If the ExtUI needs
+ // permanent data to be stored, it can write up to eeprom_data_size bytes
+ // into buff.
- // No idea why it does this twice.
- write_to_lcd_P(PSTR("{SYS:STARTED}\r\n"));
- update_usb_status(true);
-}
+ // Example:
+ // static_assert(sizeof(myDataStruct) <= ExtUI::eeprom_data_size);
+ // memcpy(buff, &myDataStruct, sizeof(myDataStruct));
+ }
-/**
- * Set an alert.
- */
-void MarlinUI::set_alert_status_P(PGM_P const message) {
- write_to_lcd_P(PSTR("{E:"));
- write_to_lcd_P(message);
- write_to_lcd_P("}");
+ void onLoadSettings(const char *buff) {
+ // This is called while loading settings from EEPROM. If the ExtUI
+ // needs to retrieve data, it should copy up to eeprom_data_size bytes
+ // from buff
+
+ // Example:
+ // static_assert(sizeof(myDataStruct) <= ExtUI::eeprom_data_size);
+ // memcpy(&myDataStruct, buff, sizeof(myDataStruct));
+ }
+
+ void onConfigurationStoreWritten(bool success) {
+ // This is called after the entire EEPROM has been written,
+ // whether successful or not.
+ }
+
+ void onConfigurationStoreRead(bool success) {
+ // This is called after the entire EEPROM has been read,
+ // whether successful or not.
+ }
}
#endif // MALYAN_LCD
diff --git a/marlin-original-clean/Marlin/src/lcd/menu/menu_backlash.cpp b/marlin-qq-clean/Marlin/src/lcd/menu/menu_backlash.cpp
index 1b183e1..9c51230 100644
--- a/marlin-original-clean/Marlin/src/lcd/menu/menu_backlash.cpp
+++ b/marlin-qq-clean/Marlin/src/lcd/menu/menu_backlash.cpp
@@ -30,26 +30,21 @@
#include "menu.h"
-extern float backlash_distance_mm[XYZ];
-extern uint8_t backlash_correction;
-
-#ifdef BACKLASH_SMOOTHING_MM
- extern float backlash_smoothing_mm;
-#endif
+#include "../../feature/backlash.h"
void menu_backlash() {
START_MENU();
MENU_BACK(MSG_MAIN);
- MENU_MULTIPLIER_ITEM_EDIT(percent, MSG_BACKLASH_CORRECTION, &backlash_correction, all_off, all_on);
+ MENU_MULTIPLIER_ITEM_EDIT(percent, MSG_BACKLASH_CORRECTION, &backlash.correction, all_off, all_on);
- #define EDIT_BACKLASH_DISTANCE(N) MENU_MULTIPLIER_ITEM_EDIT(float43, MSG_##N, &backlash_distance_mm[_AXIS(N)], 0.0f, 9.9f);
+ #define EDIT_BACKLASH_DISTANCE(N) MENU_MULTIPLIER_ITEM_EDIT(float43, MSG_##N, &backlash.distance_mm[_AXIS(N)], 0.0f, 9.9f);
EDIT_BACKLASH_DISTANCE(A);
EDIT_BACKLASH_DISTANCE(B);
EDIT_BACKLASH_DISTANCE(C);
#ifdef BACKLASH_SMOOTHING_MM
- MENU_MULTIPLIER_ITEM_EDIT(float43, MSG_BACKLASH_SMOOTHING, &backlash_smoothing_mm, 0.0f, 9.9f);
+ MENU_MULTIPLIER_ITEM_EDIT(float43, MSG_BACKLASH_SMOOTHING, &backlash.smoothing_mm, 0.0f, 9.9f);
#endif
END_MENU();
diff --git a/marlin-original-clean/Marlin/src/lcd/ultralcd.cpp b/marlin-qq-clean/Marlin/src/lcd/ultralcd.cpp
index 3fd483a..e107960 100644
--- a/marlin-original-clean/Marlin/src/lcd/ultralcd.cpp
+++ b/marlin-qq-clean/Marlin/src/lcd/ultralcd.cpp
@@ -23,7 +23,7 @@
#include "../inc/MarlinConfigPre.h"
// These displays all share the MarlinUI class
-#if HAS_SPI_LCD || EITHER(MALYAN_LCD, EXTENSIBLE_UI)
+#if HAS_SPI_LCD || ENABLED(EXTENSIBLE_UI)
#include "ultralcd.h"
#include "fontutils.h"
MarlinUI ui;
@@ -95,6 +95,12 @@
#if HAS_SLOW_BUTTONS
volatile uint8_t MarlinUI::slow_buttons;
#endif
+ #if defined(TOUCH_BUTTONS)
+ #include "xpt2046.h"
+ volatile uint8_t MarlinUI::touch_buttons;
+ uint8_t MarlinUI::read_touch_buttons() { return xpt2046_read_buttons(); }
+ #endif
+
#endif
#if ENABLED(SDSUPPORT) && PIN_EXISTS(SD_DETECT)
@@ -287,6 +293,9 @@ void MarlinUI::init() {
#if HAS_ENCODER_ACTION && HAS_SLOW_BUTTONS
slow_buttons = 0;
#endif
+ #if HAS_ENCODER_ACTION && defined(TOUCH_BUTTONS)
+ touch_buttons = 0;
+ #endif
update_buttons();
@@ -770,6 +779,9 @@ void MarlinUI::update() {
#if HAS_SLOW_BUTTONS
slow_buttons = read_slow_buttons(); // Buttons that take too long to read in interrupt context
#endif
+ #if defined(TOUCH_BUTTONS)
+ touch_buttons = read_touch_buttons();
+ #endif
#if ENABLED(REPRAPWORLD_KEYPAD)
@@ -1112,6 +1124,9 @@ void MarlinUI::update() {
#if HAS_SLOW_BUTTONS
| slow_buttons
#endif
+ #if defined(TOUCH_BUTTONS)
+ | touch_buttons
+ #endif
;
#elif HAS_ADC_BUTTONS
buttons = 0;
diff --git a/marlin-original-clean/Marlin/src/lcd/ultralcd.h b/marlin-qq-clean/Marlin/src/lcd/ultralcd.h
index 2ea7f45..965bce8 100644
--- a/marlin-original-clean/Marlin/src/lcd/ultralcd.h
+++ b/marlin-qq-clean/Marlin/src/lcd/ultralcd.h
@@ -145,6 +145,7 @@
#define EN_A _BV(BLEN_A)
#define EN_B _BV(BLEN_B)
+ //#define BUTTON_EXISTS(BN) (defined(BTN_## BN) && BTN_## BN >= 0)
#define BUTTON_PRESSED(BN) !READ(BTN_## BN)
#if BUTTON_EXISTS(ENC)
@@ -496,6 +497,10 @@ public:
static volatile uint8_t slow_buttons;
static uint8_t read_slow_buttons();
#endif
+ #if defined(TOUCH_BUTTONS)
+ static volatile uint8_t touch_buttons;
+ static uint8_t read_touch_buttons();
+ #endif
static void update_buttons();
static inline bool button_pressed() { return BUTTON_CLICK(); }
#if EITHER(AUTO_BED_LEVELING_UBL, G26_MESH_VALIDATION)
diff --git a/marlin-original-clean/Marlin/src/module/configuration_store.cpp b/marlin-qq-clean/Marlin/src/module/configuration_store.cpp
index ceab713..99a135b 100644
--- a/marlin-original-clean/Marlin/src/module/configuration_store.cpp
+++ b/marlin-qq-clean/Marlin/src/module/configuration_store.cpp
@@ -37,7 +37,7 @@
*/
// Change EEPROM version if the structure changes
-#define EEPROM_VERSION "V65"
+#define EEPROM_VERSION "V66"
#define EEPROM_OFFSET 100
// Check the integrity of data offsets.
@@ -90,10 +90,16 @@
#include "../feature/pause.h"
+#if ENABLED(BACKLASH_COMPENSATION)
+ #include "../feature/backlash.h"
+#endif
+
#if HAS_FILAMENT_SENSOR
#include "../feature/runout.h"
#endif
+#include "../lcd/extensible_ui/ui_api.h"
+
#if ENABLED(EXTRA_LIN_ADVANCE_K)
extern float saved_extruder_advance_K[EXTRUDERS];
#endif
@@ -149,6 +155,7 @@ typedef struct SettingsDataStruct {
// FILAMENT_RUNOUT_SENSOR
//
bool runout_sensor_enabled; // M412 S
+ float runout_distance_mm; // M412 D
//
// ENABLE_LEVELING_FADE_HEIGHT
@@ -298,6 +305,21 @@ typedef struct SettingsDataStruct {
toolchange_settings_t toolchange_settings; // M217 S P R
#endif
+ //
+ // BACKLASH_COMPENSATION
+ //
+ float backlash_distance_mm[XYZ]; // M425 X Y Z
+ uint8_t backlash_correction; // M425 F
+ float backlash_smoothing_mm; // M425 S
+
+ //
+ // EXTENSIBLE_UI
+ //
+ #if ENABLED(EXTENSIBLE_UI)
+ // This is a significant hardware change; don't reserve space when not present
+ uint8_t extui_data[ExtUI::eeprom_data_size];
+ #endif
+
} SettingsData;
//static_assert(sizeof(SettingsData) <= E2END + 1, "EEPROM too small to contain SettingsData!");
@@ -372,6 +394,16 @@ void MarlinSettings::postprocess() {
report_current_position();
}
+#if ENABLED(PRINTCOUNTER) && ENABLED(EEPROM_SETTINGS)
+ #include "printcounter.h"
+
+ static_assert(
+ !WITHIN(STATS_EEPROM_ADDRESS, EEPROM_OFFSET, EEPROM_OFFSET + sizeof(SettingsData)) &&
+ !WITHIN(STATS_EEPROM_ADDRESS + sizeof(printStatistics), EEPROM_OFFSET, EEPROM_OFFSET + sizeof(SettingsData)),
+ "STATS_EEPROM_ADDRESS collides with EEPROM settings storage."
+ );
+#endif
+
#if ENABLED(SD_FIRMWARE_UPDATE)
#if ENABLED(EEPROM_SETTINGS)
@@ -528,11 +560,18 @@ void MarlinSettings::postprocess() {
//
{
#if HAS_FILAMENT_SENSOR
- EEPROM_WRITE(runout.enabled);
+ const bool &runout_sensor_enabled = runout.enabled;
#else
- const bool runout_sensor_enabled = true;
- EEPROM_WRITE(runout_sensor_enabled);
+ const bool runout_sensor_enabled = false;
#endif
+ #if HAS_FILAMENT_SENSOR && defined(FILAMENT_RUNOUT_DISTANCE_MM)
+ const float &runout_distance_mm = runout.runout_distance();
+ #else
+ const float runout_distance_mm = 0;
+ #endif
+ _FIELD_TEST(runout_sensor_enabled);
+ EEPROM_WRITE(runout_sensor_enabled);
+ EEPROM_WRITE(runout_distance_mm);
}
//
@@ -1118,6 +1157,42 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(toolchange_settings);
#endif
+ //
+ // Backlash Compensation
+ //
+ {
+ #if ENABLED(BACKLASH_COMPENSATION)
+ const float (&backlash_distance_mm)[XYZ] = backlash.distance_mm;
+ const uint8_t &backlash_correction = backlash.correction;
+ #else
+ const float backlash_distance_mm[XYZ] = { 0 };
+ const uint8_t backlash_correction = 0;
+ #endif
+ #ifdef BACKLASH_SMOOTHING_MM
+ const float &backlash_smoothing_mm = backlash.smoothing_mm;
+ #else
+ const float backlash_smoothing_mm = 3;
+ #endif
+ _FIELD_TEST(backlash_distance_mm);
+ EEPROM_WRITE(backlash_distance_mm[X_AXIS]);
+ EEPROM_WRITE(backlash_distance_mm[Y_AXIS]);
+ EEPROM_WRITE(backlash_distance_mm[Z_AXIS]);
+ EEPROM_WRITE(backlash_correction);
+ EEPROM_WRITE(backlash_smoothing_mm);
+ }
+
+ //
+ // Extensible UI User Data
+ //
+ #if ENABLED(EXTENSIBLE_UI)
+ {
+ char extui_data[ExtUI::eeprom_data_size] = { 0 };
+ ExtUI::onStoreSettings(extui_data);
+ _FIELD_TEST(extui_data);
+ EEPROM_WRITE(extui_data);
+ }
+ #endif
+
//
// Validate CRC and Data Size
//
@@ -1148,7 +1223,7 @@ void MarlinSettings::postprocess() {
#endif
#if ENABLED(EXTENSIBLE_UI)
- if (!eeprom_error) ExtUI::onStoreSettings();
+ ExtUI::onConfigurationStoreWritten(!eeprom_error);
#endif
return !eeprom_error;
@@ -1264,12 +1339,18 @@ void MarlinSettings::postprocess() {
// Filament Runout Sensor
//
{
- _FIELD_TEST(runout_sensor_enabled);
#if HAS_FILAMENT_SENSOR
- EEPROM_READ(runout.enabled);
+ bool &runout_sensor_enabled = runout.enabled;
#else
bool runout_sensor_enabled;
- EEPROM_READ(runout_sensor_enabled);
+ #endif
+ _FIELD_TEST(runout_sensor_enabled);
+ EEPROM_READ(runout_sensor_enabled);
+
+ float runout_distance_mm;
+ EEPROM_READ(runout_distance_mm);
+ #if HAS_FILAMENT_SENSOR && defined(FILAMENT_RUNOUT_DISTANCE_MM)
+ runout.set_runout_distance(runout_distance_mm);
#endif
}
@@ -1851,6 +1932,44 @@ void MarlinSettings::postprocess() {
EEPROM_READ(toolchange_settings);
#endif
+ //
+ // Backlash Compensation
+ //
+ {
+ #if ENABLED(BACKLASH_COMPENSATION)
+ float (&backlash_distance_mm)[XYZ] = backlash.distance_mm;
+ uint8_t &backlash_correction = backlash.correction;
+ #else
+ float backlash_distance_mm[XYZ];
+ uint8_t backlash_correction;
+ #endif
+ #ifdef BACKLASH_SMOOTHING_MM
+ float &backlash_smoothing_mm = backlash.smoothing_mm;
+ #else
+ float backlash_smoothing_mm;
+ #endif
+ _FIELD_TEST(backlash_distance_mm);
+ EEPROM_READ(backlash_distance_mm[X_AXIS]);
+ EEPROM_READ(backlash_distance_mm[Y_AXIS]);
+ EEPROM_READ(backlash_distance_mm[Z_AXIS]);
+ EEPROM_READ(backlash_correction);
+ EEPROM_READ(backlash_smoothing_mm);
+ }
+
+ //
+ // Extensible UI User Data
+ //
+ #if ENABLED(EXTENSIBLE_UI)
+ // This is a significant hardware change; don't reserve EEPROM space when not present
+ {
+ const char extui_data[ExtUI::eeprom_data_size] = { 0 };
+ _FIELD_TEST(extui_data);
+ EEPROM_READ(extui_data);
+ if(!validating)
+ ExtUI::onLoadSettings(extui_data);
+ }
+ #endif
+
eeprom_error = size_error(eeprom_index - (EEPROM_OFFSET));
if (eeprom_error) {
DEBUG_ECHO_START();
@@ -1921,7 +2040,7 @@ void MarlinSettings::postprocess() {
if (validate()) {
const bool success = _load();
#if ENABLED(EXTENSIBLE_UI)
- if (success) ExtUI::onLoadSettings();
+ ExtUI::onConfigurationStoreRead(success);
#endif
return success;
}
@@ -2053,13 +2172,13 @@ void MarlinSettings::reset() {
#if HAS_CLASSIC_JERK
#ifndef DEFAULT_XJERK
- #define DEFAULT_XJERK 0
+ #define DEFAULT_XJERK 10
#endif
#ifndef DEFAULT_YJERK
- #define DEFAULT_YJERK 0
+ #define DEFAULT_YJERK 10
#endif
#ifndef DEFAULT_ZJERK
- #define DEFAULT_ZJERK 0
+ #define DEFAULT_ZJERK 10
#endif
planner.max_jerk[X_AXIS] = DEFAULT_XJERK;
planner.max_jerk[Y_AXIS] = DEFAULT_YJERK;
@@ -2090,6 +2209,9 @@ void MarlinSettings::reset() {
#if HAS_FILAMENT_SENSOR
runout.enabled = true;
runout.reset();
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ runout.set_runout_distance(FILAMENT_RUNOUT_DISTANCE_MM);
+ #endif
#endif
//
@@ -2108,6 +2230,23 @@ void MarlinSettings::reset() {
toolchange_settings.z_raise = TOOLCHANGE_ZRAISE;
#endif
+ #if ENABLED(BACKLASH_GCODE)
+ backlash.correction = (BACKLASH_CORRECTION) * 255;
+ #ifdef BACKLASH_DISTANCE_MM
+ constexpr float tmp[XYZ] = BACKLASH_DISTANCE_MM;
+ backlash.distance_mm[X_AXIS] = tmp[X_AXIS];
+ backlash.distance_mm[Y_AXIS] = tmp[Y_AXIS];
+ backlash.distance_mm[Z_AXIS] = tmp[Z_AXIS];
+ #endif
+ #ifdef BACKLASH_SMOOTHING_MM
+ backlash.smoothing_mm = BACKLASH_SMOOTHING_MM;
+ #endif
+ #endif
+
+ #if ENABLED(EXTENSIBLE_UI)
+ ExtUI::onFactoryReset();
+ #endif
+
//
// Magnetic Parking Extruder
//
@@ -3200,6 +3339,31 @@ void MarlinSettings::reset() {
CONFIG_ECHO_START();
M217_report(true);
#endif
+
+ #if ENABLED(BACKLASH_GCODE)
+ CONFIG_ECHO_HEADING("Backlash compensation:");
+ CONFIG_ECHO_START();
+ SERIAL_ECHOLNPAIR(
+ " M425 F", backlash.get_correction(),
+ " X", LINEAR_UNIT(backlash.distance_mm[X_AXIS]),
+ " Y", LINEAR_UNIT(backlash.distance_mm[Y_AXIS]),
+ " Z", LINEAR_UNIT(backlash.distance_mm[Z_AXIS])
+ #ifdef BACKLASH_SMOOTHING_MM
+ , " S", LINEAR_UNIT(backlash.smoothing_mm)
+ #endif
+ );
+ #endif
+
+ #if HAS_FILAMENT_SENSOR
+ CONFIG_ECHO_HEADING("Filament runout sensor:");
+ CONFIG_ECHO_START();
+ SERIAL_ECHOLNPAIR(
+ " M412 S", int(runout.enabled)
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+ , " D", LINEAR_UNIT(runout.runout_distance())
+ #endif
+ );
+ #endif
}
#endif // !DISABLE_M503
diff --git a/marlin-original-clean/Marlin/src/module/planner.cpp b/marlin-qq-clean/Marlin/src/module/planner.cpp
index 5073d90..9c8a6f0 100644
--- a/marlin-original-clean/Marlin/src/module/planner.cpp
+++ b/marlin-qq-clean/Marlin/src/module/planner.cpp
@@ -92,6 +92,10 @@
#include "../feature/power.h"
#endif
+#if ENABLED(BACKLASH_COMPENSATION)
+ #include "../feature/backlash.h"
+#endif
+
// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
#define BLOCK_DELAY_FOR_1ST_MOVE 100
@@ -1560,94 +1564,6 @@ void Planner::synchronize() {
) idle();
}
-/**
- * The following implements axis backlash correction. To minimize seams
- * on the printed part, the backlash correction only adds steps to the
- * current segment (instead of creating a new segment, which causes
- * discontinuities and print artifacts).
- *
- * When BACKLASH_SMOOTHING_MM is enabled and non-zero, the backlash
- * correction is spread over multiple segments, smoothing out print
- * artifacts even more.
- */
-#if ENABLED(BACKLASH_COMPENSATION)
- #if ENABLED(BACKLASH_GCODE)
- extern float backlash_distance_mm[];
- extern uint8_t backlash_correction;
- #ifdef BACKLASH_SMOOTHING_MM
- extern float backlash_smoothing_mm;
- #endif
- #else
- constexpr float backlash_distance_mm[XYZ] = BACKLASH_DISTANCE_MM,
- constexpr uint8_t backlash_correction = BACKLASH_CORRECTION * 255;
- #ifdef BACKLASH_SMOOTHING_MM
- constexpr float backlash_smoothing_mm = BACKLASH_SMOOTHING_MM;
- #endif
- #endif
-
- void Planner::add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block) {
- static uint8_t last_direction_bits;
- uint8_t changed_dir = last_direction_bits ^ dm;
- // Ignore direction change if no steps are taken in that direction
- if (da == 0) CBI(changed_dir, X_AXIS);
- if (db == 0) CBI(changed_dir, Y_AXIS);
- if (dc == 0) CBI(changed_dir, Z_AXIS);
- last_direction_bits ^= changed_dir;
-
- if (backlash_correction == 0) return;
-
- #ifdef BACKLASH_SMOOTHING_MM
- // The segment proportion is a value greater than 0.0 indicating how much residual_error
- // is corrected for in this segment. The contribution is based on segment length and the
- // smoothing distance. Since the computation of this proportion involves a floating point
- // division, defer computation until needed.
- float segment_proportion = 0;
-
- // Residual error carried forward across multiple segments, so correction can be applied
- // to segments where there is no direction change.
- static int32_t residual_error[XYZ] = { 0 };
- #else
- // No leftover residual error from segment to segment
- int32_t residual_error[XYZ] = { 0 };
- // No direction change, no correction.
- if (!changed_dir) return;
- #endif
-
- const float f_corr = float(backlash_correction) / 255.0f;
-
- LOOP_XYZ(axis) {
- if (backlash_distance_mm[axis]) {
- const bool reversing = TEST(dm,axis);
-
- // When an axis changes direction, add axis backlash to the residual error
- if (TEST(changed_dir, axis))
- residual_error[axis] += (reversing ? -f_corr : f_corr) * backlash_distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
-
- // Decide how much of the residual error to correct in this segment
- int32_t error_correction = residual_error[axis];
- #ifdef BACKLASH_SMOOTHING_MM
- if (error_correction && backlash_smoothing_mm != 0) {
- // Take up a portion of the residual_error in this segment, but only when
- // the current segment travels in the same direction as the correction
- if (reversing == (error_correction < 0)) {
- if (segment_proportion == 0)
- segment_proportion = MIN(1.0f, block->millimeters / backlash_smoothing_mm);
- error_correction = ceil(segment_proportion * error_correction);
- }
- else
- error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps
- }
- #endif
- // Making a correction reduces the residual error and modifies delta_mm
- if (error_correction) {
- block->steps[axis] += ABS(error_correction);
- residual_error[axis] -= error_correction;
- }
- }
- }
- }
-#endif // BACKLASH_COMPENSATION
-
/**
* Planner::_buffer_steps
*
@@ -1919,7 +1835,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
* should *never* remove steps!
*/
#if ENABLED(BACKLASH_COMPENSATION)
- add_backlash_correction_steps(da, db, dc, dm, block);
+ backlash.add_correction_steps(da, db, dc, dm, block);
#endif
}
diff --git a/marlin-original-clean/Marlin/src/module/planner.h b/marlin-qq-clean/Marlin/src/module/planner.h
index a95c3f2..5e24ce6 100644
--- a/marlin-original-clean/Marlin/src/module/planner.h
+++ b/marlin-qq-clean/Marlin/src/module/planner.h
@@ -338,10 +338,6 @@ class Planner {
volatile static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs
#endif
- #if ENABLED(BACKLASH_COMPENSATION)
- static void add_backlash_correction_steps(const int32_t da, const int32_t db, const int32_t dc, const uint8_t dm, block_t * const block);
- #endif
-
public:
/**
diff --git a/marlin-original-clean/Marlin/src/module/printcounter.cpp b/marlin-qq-clean/Marlin/src/module/printcounter.cpp
index 4fa3e50..51af80a 100644
--- a/marlin-original-clean/Marlin/src/module/printcounter.cpp
+++ b/marlin-qq-clean/Marlin/src/module/printcounter.cpp
@@ -29,6 +29,10 @@ Stopwatch print_job_timer; // Global Print Job Timer instance
#else // PRINTCOUNTER
+#if ENABLED(EXTENSIBLE_UI)
+ #include "../lcd/extensible_ui/ui_api.h"
+#endif
+
#include "printcounter.h"
#include "../Marlin.h"
#include "../HAL/shared/persistent_store_api.h"
@@ -169,6 +173,10 @@ void PrintCounter::saveStats() {
persistentStore.access_start();
persistentStore.write_data(address + sizeof(uint8_t), (uint8_t*)&data, sizeof(printStatistics));
persistentStore.access_finish();
+
+ #if ENABLED(EXTENSIBLE_UI)
+ ExtUI::onConfigurationStoreWritten(true);
+ #endif
}
#if HAS_SERVICE_INTERVALS
diff --git a/marlin-original-clean/Marlin/src/module/probe.cpp b/marlin-qq-clean/Marlin/src/module/probe.cpp
index 8988b15..39b6c8e 100644
--- a/marlin-original-clean/Marlin/src/module/probe.cpp
+++ b/marlin-qq-clean/Marlin/src/module/probe.cpp
@@ -54,6 +54,10 @@
#include "planner.h"
#endif
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+ #include "../feature/backlash.h"
+#endif
+
float zprobe_zoffset; // Initialized by settings.load()
#if ENABLED(BLTOUCH)
@@ -463,30 +467,6 @@ bool set_probe_deployed(const bool deploy) {
}
#endif
-#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
- #if USES_Z_MIN_PROBE_ENDSTOP
- #define TEST_PROBE_PIN (READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING)
- #else
- #define TEST_PROBE_PIN (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)
- #endif
-
- extern float backlash_measured_mm[];
- extern uint8_t backlash_measured_num[];
-
- /* Measure Z backlash by raising nozzle in increments until probe deactivates */
- static void measure_backlash_with_probe() {
- if (backlash_measured_num[Z_AXIS] == 255) return;
-
- float start_height = current_position[Z_AXIS];
- while (current_position[Z_AXIS] < (start_height + BACKLASH_MEASUREMENT_LIMIT) && TEST_PROBE_PIN)
- do_blocking_move_to_z(current_position[Z_AXIS] + BACKLASH_MEASUREMENT_RESOLUTION, MMM_TO_MMS(BACKLASH_MEASUREMENT_FEEDRATE));
-
- // The backlash from all probe points is averaged, so count the number of measurements
- backlash_measured_mm[Z_AXIS] += current_position[Z_AXIS] - start_height;
- backlash_measured_num[Z_AXIS]++;
- }
-#endif
-
/**
* @brief Used by run_z_probe to do a single Z probe move.
*
@@ -643,7 +623,7 @@ static float run_z_probe() {
}
#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
- measure_backlash_with_probe();
+ backlash.measure_with_probe();
#endif
#if MULTIPLE_PROBING > 2
diff --git a/marlin-original-clean/Marlin/src/module/stepper.cpp b/marlin-qq-clean/Marlin/src/module/stepper.cpp
index ce3141c..0e758e9 100644
--- a/marlin-original-clean/Marlin/src/module/stepper.cpp
+++ b/marlin-qq-clean/Marlin/src/module/stepper.cpp
@@ -113,7 +113,7 @@ Stepper stepper; // Singleton
#include "../feature/mixing.h"
#endif
-#if FILAMENT_RUNOUT_DISTANCE_MM > 0
+#ifdef FILAMENT_RUNOUT_DISTANCE_MM
#include "../feature/runout.h"
#endif
@@ -1537,7 +1537,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
// If current block is finished, reset pointer
if (step_events_completed >= step_event_count) {
- #if FILAMENT_RUNOUT_DISTANCE_MM > 0
+ #ifdef FILAMENT_RUNOUT_DISTANCE_MM
runout.block_completed(current_block);
#endif
axis_did_move = 0;
@@ -2579,6 +2579,12 @@ void Stepper::report_positions() {
#ifdef __AVR__
SET_CS5(PRESCALER_1);
#endif
+ // Set TIM3 to 36khz. Prescaler is 2000.
+ #ifdef __STM32F1__
+ #if MB(MKS_ROBIN_MINI)
+ PWM_vref_init();
+ #endif
+ #endif
#endif
}
diff --git a/marlin-original-clean/Marlin/src/module/thermistor/thermistor_666.h b/marlin-qq-clean/Marlin/src/module/thermistor/thermistor_666.h
old mode 100755
new mode 100644
diff --git a/marlin-original-clean/Marlin/src/pins/pins.h b/marlin-qq-clean/Marlin/src/pins/pins.h
index bb7a101..d820a17 100644
--- a/marlin-original-clean/Marlin/src/pins/pins.h
+++ b/marlin-qq-clean/Marlin/src/pins/pins.h
@@ -422,6 +422,8 @@
#include "pins_MORPHEUS.h" // STM32F1 env:STM32F1
#elif MB(MKS_ROBIN)
#include "pins_MKS_ROBIN.h" // STM32F1 env:mks_robin
+#elif MB(MKS_ROBIN_MINI)
+ #include "pins_MKS_ROBIN_MINI.h" // STM32F1 env:mks_robin_mini
//
// STM32 ARM Cortex-M4F
diff --git a/marlin-original-clean/Marlin/src/pins/pins_5DPRINT.h b/marlin-qq-clean/Marlin/src/pins/pins_5DPRINT.h
old mode 100755
new mode 100644
diff --git a/marlin-original-clean/Marlin/src/pins/pins_AZTEEG_X5_GT.h b/marlin-qq-clean/Marlin/src/pins/pins_AZTEEG_X5_GT.h
old mode 100755
new mode 100644
diff --git a/marlin-original-clean/Marlin/src/pins/pins_EINSTART-S.h b/marlin-qq-clean/Marlin/src/pins/pins_EINSTART-S.h
old mode 100755
new mode 100644
diff --git a/marlin-qq-clean/Marlin/src/pins/pins_MKS_ROBIN_MINI.h b/marlin-qq-clean/Marlin/src/pins/pins_MKS_ROBIN_MINI.h
new file mode 100644
index 0000000..5de0dff
--- /dev/null
+++ b/marlin-qq-clean/Marlin/src/pins/pins_MKS_ROBIN_MINI.h
@@ -0,0 +1,151 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * MKS Robin (STM32F130ZET6) board pin assignments
+ */
+
+#ifndef __STM32F1__
+ #error "Oops! Select an STM32F1 board in 'Tools > Board.'"
+#endif
+
+#if HOTENDS > 1 || E_STEPPERS > 1
+ #error "MKS Robin mini supports only 1 hotend / E-stepper. Comment out this line to continue."
+#endif
+
+#define BOARD_NAME "MKS Robin mini"
+
+//
+// Release PB4 (Y_ENABLE_PIN) from JTAG NRST role
+//
+#define DISABLE_DEBUG
+
+//
+// Note: MKS Robin mini board is using SPI2 interface.
+//
+#define SPI_MODULE 2
+//#define I2C_EEPROM
+#define FLASH_EEPROM_EMULATION
+#define E2END 0x800 // 2K in a 24C16C
+
+// This is for EEPROM emulation in flash
+#define EEPROM_PAGE_SIZE (uint16)0x800 // 2048
+#define EEPROM_START_ADDRESS ((uint32)(0x8000000 + 512 * 1024 - 2 * EEPROM_PAGE_SIZE))
+#define EEPROM_PAGE0_BASE ((uint32)(EEPROM_START_ADDRESS + 0x000))
+#define EEPROM_PAGE1_BASE ((uint32)(EEPROM_START_ADDRESS + EEPROM_PAGE_SIZE))
+#define EEPROM_SIZE ((uint16)(EEPROM_PAGE_SIZE * 2))
+
+//
+// Servos
+// MKS Robin mini have no servos interface
+//#define SERVO0_PIN PC3 // XS1 - 5
+//#define SERVO1_PIN PA1 // XS1 - 6
+//#define SERVO2_PIN PF9 // XS2 - 5
+//#define SERVO3_PIN PF8 // XS2 - 6
+
+//
+// Limit Switches
+//
+#define X_MAX_PIN PA15
+#define Y_MAX_PIN PA12
+#define Z_MIN_PIN PA11
+#define Z_MAX_PIN PC4
+
+//
+// Steppers
+//
+#define X_ENABLE_PIN PE4
+#define X_STEP_PIN PE3
+#define X_DIR_PIN PE2
+
+#define Y_ENABLE_PIN PE1
+#define Y_STEP_PIN PE0
+#define Y_DIR_PIN PB9
+
+#define Z_ENABLE_PIN PB8
+#define Z_STEP_PIN PB5
+#define Z_DIR_PIN PB4
+
+#define E0_ENABLE_PIN PB3
+#define E0_STEP_PIN PD6
+#define E0_DIR_PIN PD3
+
+//
+// Temperature Sensors
+//
+#define TEMP_0_PIN PC1 // TH1
+#define TEMP_BED_PIN PC0 // TB1
+
+
+//
+// Heaters / Fans
+//
+#define HEATER_0_PIN PC3 // HEATER1
+#define HEATER_BED_PIN PA0 // HOT BED
+#define FAN_PIN PB1 // FAN
+
+#define BTN_ENC PC13 // Pin is not connected. Real pin is needed to enable encoder's push button functionality used by touch screen
+#define BTN_EN1 -1
+#define BTN_EN2 -1
+
+//#define MAX6675_SS_PIN PE5 // TC1 - CS1
+//#define MAX6675_SS_PIN PE6 // TC2 - CS2
+
+#define POWER_LOSS_PIN PA1 // PW_DET
+#define FIL_RUNOUT_PIN PA4 // MT_DET
+
+#define BEEPER_PIN PC5
+
+//#define LED_PIN PB2 //Robin mini have no status led
+
+/**
+ * Note: MKS Robin TFT screens may have different TFT controllers
+ * If the screen stays white, disable 'LCD_RESET_PIN' to rely on the bootloader to do screen initialization.
+ *
+ * Enabling 'LCD_RESET_PIN' causes flickering when entering the LCD menu due to LCD controller reset.
+ * Reset feature was designed to "revive the LCD if static electricity killed it."
+ */
+//#define LCD_RESET_PIN PC6
+#define LCD_BACKLIGHT_PIN PD13
+#define FSMC_CS_PIN PD7 // NE1
+#define FSMC_RS_PIN PD11 // A16
+#define TOUCH_CS PC2
+
+#define SD_DETECT_PIN PD12
+
+// Motor current PWM pins
+#define MOTOR_CURRENT_PWM_XY_PIN PA6
+#define MOTOR_CURRENT_PWM_Z_PIN PA7
+#define MOTOR_CURRENT_PWM_E_PIN PB0
+#define MOTOR_CURRENT_PWM_RANGE 65535 // (255 * (1000mA / 65535)) * 257 = 1000 is equal 1.6v Vref in turn equal 1Amp
+#define DEFAULT_PWM_MOTOR_CURRENT {1030, 1030, 1030} // 1.05Amp per driver, here is XY, Z and E. This values determined empirically.
+
+// this is a kind of workaround in case native marlin "digipot" interface won't work, required to uncomment related code in stm32f1/hal.cpp
+//#ifndef MKS_ROBIN_MINI_VREF_PWM
+// #define MKS_ROBIN_MINI_VREF_PWM
+//#endif
+
+//#define VREF_XY_PIN PA6
+//#define VREF_Z_PIN PA7
+//#define VREF_E1_PIN PB0
+
+
\ No newline at end of file
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment