├── firmware ├── revd.dfu ├── revh.dfu └── LICENSE ├── requirements.txt ├── install.sh ├── README.md ├── update_firmware.py ├── LICENSE └── beacon.py /firmware/revd.dfu: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/beacon3d/beacon_klipper/HEAD/firmware/revd.dfu -------------------------------------------------------------------------------- /firmware/revh.dfu: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/beacon3d/beacon_klipper/HEAD/firmware/revh.dfu -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # klipper python virtual environment requirements for beacon 2 | numpy>=1.16.6 3 | scipy>=1.2.3 4 | -------------------------------------------------------------------------------- /firmware/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (C) Beacon - All Rights Reserved. 2 | 3 | THE CONTENTS OF THIS PROJECT ARE PROPRIETARY AND CONFIDENTIAL. 4 | UNAUTHORIZED COPYING, TRANSFERRING or MODIFICATION OF THE CONTENTS OF THIS PROJECT, VIA ANY MEDIUM IS STRICTLY PROHIBITED. 5 | 6 | The receipt or possession of the source code, binary code and/or any parts thereof does not convey or imply any right to use them 7 | for any purpose other than the purpose for which they were provided to you. 8 | 9 | The software is provided "AS IS", without warranty of any kind, express or implied, including but not limited to 10 | the warranties of merchantability, fitness for a particular purpose and non infringement. 11 | In no event shall the authors or copyright holders be liable for any claim, damages or other liability, 12 | whether in an action of contract, tort or otherwise, arising from, out of or in connection with the software 13 | or the use or other dealings in the software. 14 | 15 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 16 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | KDIR="${HOME}/klipper" 4 | KENV="${HOME}/klippy-env" 5 | 6 | BKDIR="$( cd -- "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" 7 | 8 | if [ ! -d "$KDIR" ] || [ ! -d "$KENV" ]; then 9 | echo "beacon: klipper or klippy env doesn't exist" 10 | exit 1 11 | fi 12 | 13 | # install beacon requirements to env 14 | echo "beacon: installing python requirements to env, this may take 10+ minutes." 15 | "${KENV}/bin/pip" install -r "${BKDIR}/requirements.txt" 16 | 17 | # update link to beacon.py 18 | echo "beacon: linking klippy to beacon.py." 19 | if [ -e "${KDIR}/klippy/extras/beacon.py" ]; then 20 | rm "${KDIR}/klippy/extras/beacon.py" 21 | fi 22 | ln -s "${BKDIR}/beacon.py" "${KDIR}/klippy/extras/beacon.py" 23 | 24 | # exclude beacon.py from klipper git tracking 25 | if ! grep -q "klippy/extras/beacon.py" "${KDIR}/.git/info/exclude"; then 26 | echo "klippy/extras/beacon.py" >> "${KDIR}/.git/info/exclude" 27 | fi 28 | echo "beacon: installation successful." 29 | 30 | echo "Updating firmware." 31 | "$KENV/bin/python" "$BKDIR/update_firmware.py" update all 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Beacon Klipper 2 | 3 | Beacon Klipper is the klipper module for using the [Beacon](https://beacon3d.com) Eddy Current Scanner. 4 | 5 | ## Documentation 6 | 7 | [Beacon](https://docs.beacon3d.com) 8 | 9 | ## Firmware Release Notes 10 | 11 | ### Beacon 2.1.0 - July 11, 2024 12 | - Added parameters to adjust contact noise tolerance 13 | - Adjusted contact latency values to match new parameters 14 | - Increased robustness of the primary contact trigger 15 | 16 | ### Beacon 2.0.1 - June 4, 2024 17 | - Fixed USB enumeration issue affecting fast host controllers 18 | 19 | ### Beacon 2.0.0 - May 29, 2024 20 | - Beacon Contact Release 21 | - Adopted RTIC - The Hardware Accelerated Rust RTOS 22 | - Added nozzle contact detection processing 23 | - Improved data transmit and processing efficiency 24 | - Reports MCU temperature and supply voltage 25 | - Added watchdog superviser 26 | - Improved error detection, reporting, and recovery 27 | - Reduced current consumption 10% overall 28 | - Reduced current consumption 55% when used above rated temperature 29 | 30 | ### Beacon 1.1.0 - Dec 27, 2023 31 | - RevH Enabling Release 32 | - Added Accel Driver 33 | 34 | ### Beacon 1.0.0 - Jan 26, 2023 35 | - Initial Release 36 | 37 | -------------------------------------------------------------------------------- /update_firmware.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import os 5 | import sys 6 | import termios 7 | import time 8 | import subprocess 9 | import struct 10 | 11 | SELF_DIR = os.path.dirname(__file__) 12 | 13 | 14 | def serial_sys_devpath(device_path): 15 | device_name = os.path.basename(os.path.realpath(device_path)) 16 | try: 17 | sys_path = os.path.realpath( 18 | os.path.join("/sys/class/tty/", device_name, "device", "..") 19 | ) 20 | except OSError as e: 21 | return None 22 | return sys_path 23 | 24 | 25 | def read_dev_file(devpath, file): 26 | target = os.path.join(devpath, file) 27 | if not os.path.exists(target): 28 | return None 29 | with open(target) as f: 30 | return f.read().strip() 31 | 32 | 33 | def check_device_is_beacon(devpath): 34 | manufacturer = read_dev_file(devpath, "manufacturer") 35 | vendor = read_dev_file(devpath, "idVendor") 36 | if manufacturer != "Beacon" and vendor != "04d8": 37 | return False 38 | product = read_dev_file(devpath, "product") 39 | if product is None or not product.startswith("Beacon "): 40 | return False 41 | rev = product[7:].lower() 42 | if not rev.startswith("rev"): 43 | return False 44 | return rev[3:].split(" ")[0] 45 | 46 | 47 | def get_device_fw_version(devpath): 48 | raw = read_dev_file(devpath, "bcdDevice") 49 | return (int(raw[0]), int(raw[1] + raw[2]), int(raw[3])) 50 | 51 | 52 | def fw_path(rev): 53 | return os.path.join(SELF_DIR, "firmware", "rev" + rev + ".dfu") 54 | 55 | 56 | def get_fw_file_version(fwpath): 57 | file_size = os.stat(fwpath).st_size 58 | with open(fwpath, "rb") as f: 59 | f.seek(file_size - 16) 60 | file_version = bytes(f.read(2)) 61 | (lo, hi) = struct.unpack("BB", file_version) 62 | return (((hi >> 4) & 0xF), (hi & 0xF) * 10 + ((lo >> 4) & 0xF), (lo & 0xF)) 63 | 64 | 65 | def format_fw_version(version): 66 | return "%d.%d.%d" % version 67 | 68 | 69 | def enter_bootloader(device_path): 70 | with open(device_path, "rb") as f: 71 | fd = f.fileno() 72 | t = termios.tcgetattr(fd) 73 | t[4] = t[5] = termios.B1200 74 | termios.tcsetattr(fd, termios.TCSANOW, t) 75 | 76 | 77 | def wait_bootloader(sys_path): 78 | timeout = time.time() + 5 79 | while 1: 80 | time.sleep(0.1) 81 | if not os.path.exists(sys_path): 82 | continue 83 | product = read_dev_file(sys_path, "product") 84 | if "Bootloader" in product: 85 | return 86 | if time.time() >= timeout: 87 | return 88 | 89 | 90 | def flash(fw_path, sudo): 91 | args = ["dfu-util", "-e", "-D", fw_path] 92 | if sudo: 93 | args.insert(0, "sudo") 94 | print("Running dfu-util: %s" % (" ".join(args),)) 95 | p = subprocess.Popen(args, stderr=subprocess.PIPE) 96 | out, err = p.communicate() 97 | if p.returncode != 0: 98 | err = err.decode() 99 | if ( 100 | not "Error sending completion packet" in err 101 | and not "unable to read DFU status after completion" in err 102 | ): 103 | print(err) 104 | raise Exception("Flashing failed, dfu-util returned %d" % (p.returncode,)) 105 | else: 106 | print("\nDownload done.") 107 | 108 | 109 | def do_update(device_path, sys_path, rev, no_sudo, force): 110 | if device_path is not None: 111 | actual_fw_version = format_fw_version(get_device_fw_version(sys_path)) 112 | desired_fw_version = format_fw_version(get_fw_file_version(fw_path(rev))) 113 | if not force and actual_fw_version == desired_fw_version: 114 | serial = read_dev_file(sys_path, "serial") 115 | print( 116 | "Beacon '%s' is already flashed with the current firmware version '%s'. Skipping.\n" 117 | "To force update, re-run `update_firmware.py` with the `--force` flag set." 118 | % (serial, actual_fw_version) 119 | ) 120 | return 121 | 122 | enter_bootloader(device_path) 123 | wait_bootloader(sys_path) 124 | flash(os.path.join(SELF_DIR, "firmware", "rev" + rev + ".dfu"), not no_sudo) 125 | 126 | 127 | def find_beacons(): 128 | devices = [] 129 | prefix = "/sys/bus/usb/devices" 130 | for dev in os.listdir(prefix): 131 | try: 132 | sys_path = os.path.realpath(os.path.join(prefix, dev)) 133 | except OSError as e: 134 | continue 135 | rev = check_device_is_beacon(sys_path) 136 | if rev == False: 137 | continue 138 | device_path = None 139 | for sub in os.listdir(sys_path): 140 | tty_path = os.path.realpath(os.path.join(sys_path, sub, "tty")) 141 | if not os.path.exists(tty_path): 142 | continue 143 | try: 144 | device_name = os.listdir(tty_path).pop() 145 | except IndexError: 146 | continue 147 | device_path = "/dev/" + device_name 148 | break 149 | 150 | devices.append((device_path, sys_path, rev)) 151 | return devices 152 | 153 | 154 | def task_check(device_path): 155 | sys_devpath = serial_sys_devpath(device_path) 156 | if sys_devpath is None: 157 | print("Could not look up syspath for device") 158 | sys.exit(255) 159 | 160 | rev = check_device_is_beacon(sys_devpath) 161 | if rev == False: 162 | print("Device does not appear to be a Beacon") 163 | sys.exit(255) 164 | 165 | actual_fw_version = format_fw_version(get_device_fw_version(sys_devpath)) 166 | desired_fw_version = format_fw_version( 167 | get_fw_file_version(os.path.join(SELF_DIR, "firmware", "rev" + rev + ".dfu")) 168 | ) 169 | 170 | if actual_fw_version != desired_fw_version: 171 | print( 172 | "Outdated Beacon firmware version %s, current version is %s.\n" 173 | "Please run `install.sh` or `update_firmware.py update all` to update to the latest version.\n" 174 | "Using an outdated firmware version can result in instability or failures." 175 | % (actual_fw_version, desired_fw_version) 176 | ) 177 | 178 | 179 | def task_update(device_path, no_sudo, force): 180 | if device_path == "all": 181 | for device_path, sys_path, rev in find_beacons(): 182 | do_update(device_path, sys_path, rev, no_sudo, force) 183 | else: 184 | sys_devpath = serial_sys_devpath(device_path) 185 | if sys_devpath is None: 186 | print("Could not find sys entry for given device") 187 | sys.exit(255) 188 | 189 | rev = check_device_is_beacon(sys_devpath) 190 | if rev == False: 191 | print("Given device does not appear to be a Beacon") 192 | sys.exit(255) 193 | do_update(device_path, sys_devpath, rev, no_sudo, force) 194 | 195 | 196 | if __name__ == "__main__": 197 | parser = argparse.ArgumentParser(description="Beacon firmware updater") 198 | subparsers = parser.add_subparsers(dest="command") 199 | subparsers.required = True 200 | 201 | parser_check = subparsers.add_parser("check") 202 | parser_check.add_argument("device_path") 203 | 204 | parser_update = subparsers.add_parser("update") 205 | parser_update.add_argument("device_path") 206 | parser_update.add_argument("--no-sudo", default=False, action="store_true") 207 | parser_update.add_argument("--force", default=False, action="store_true") 208 | 209 | kwargs = vars(parser.parse_args()) 210 | sub = kwargs.pop("command") 211 | globals()["task_" + sub](**kwargs) 212 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /beacon.py: -------------------------------------------------------------------------------- 1 | # Beacon eddy current scanner support 2 | # 3 | # Copyright (C) 2020-2023 Matt Baker 4 | # Copyright (C) 2020-2023 Lasse Dalegaard 5 | # Copyright (C) 2023 Beacon 6 | # 7 | # This file may be distributed under the terms of the GNU GPLv3 license. 8 | import threading 9 | import multiprocessing 10 | import subprocess 11 | import os 12 | import importlib 13 | import traceback 14 | import logging 15 | import chelper 16 | import pins 17 | import math 18 | import time 19 | import queue 20 | import struct 21 | import numpy as np 22 | import copy 23 | import collections 24 | import itertools 25 | from numpy.polynomial import Polynomial 26 | from . import manual_probe 27 | from . import probe 28 | from . import bed_mesh 29 | from . import thermistor 30 | from . import adxl345 31 | from .homing import HomingMove 32 | from mcu import MCU, MCU_trsync 33 | from clocksync import SecondarySync 34 | import msgproto 35 | 36 | STREAM_BUFFER_LIMIT_DEFAULT = 100 37 | STREAM_TIMEOUT = 1.0 38 | API_DUMP_FIELDS = ["dist", "temp", "pos", "freq", "time"] 39 | 40 | TRSYNC_TIMEOUT_DEFAULT = 0.025 41 | 42 | 43 | class BeaconProbe: 44 | def __init__(self, config, sensor_id): 45 | self.id = sensor_id 46 | self.printer = printer = config.get_printer() 47 | self.reactor = printer.get_reactor() 48 | self.name = config.get_name() 49 | self.gcode = printer.lookup_object("gcode") 50 | 51 | self.speed = config.getfloat("speed", 5.0, above=0.0) 52 | self.lift_speed = config.getfloat("lift_speed", self.speed, above=0.0) 53 | self.backlash_comp = config.getfloat("backlash_comp", 0.5) 54 | 55 | self.x_offset = config.getfloat("x_offset", 0.0) 56 | self.y_offset = config.getfloat("y_offset", 0.0) 57 | 58 | self.trigger_distance = config.getfloat("trigger_distance", 2.0) 59 | self.trigger_dive_threshold = config.getfloat("trigger_dive_threshold", 1.0) 60 | self.trigger_hysteresis = config.getfloat("trigger_hysteresis", 0.006) 61 | self.z_settling_time = config.getint("z_settling_time", 5, minval=0) 62 | self.default_probe_method = config.getchoice( 63 | "default_probe_method", 64 | {"contact": "contact", "proximity": "proximity"}, 65 | "proximity", 66 | ) 67 | 68 | # If using paper for calibration, this would be .1mm 69 | self.cal_nozzle_z = config.getfloat("cal_nozzle_z", 0.1) 70 | self.cal_floor = config.getfloat("cal_floor", 0.2) 71 | self.cal_ceil = config.getfloat("cal_ceil", 5.0) 72 | self.cal_speed = config.getfloat("cal_speed", 1.0) 73 | self.cal_move_speed = config.getfloat("cal_move_speed", 10.0) 74 | 75 | self.autocal_max_speed = config.getfloat("autocal_max_speed", 10) 76 | self.autocal_speed = config.getfloat("autocal_speed", 3) 77 | self.autocal_accel = config.getfloat("autocal_accel", 100) 78 | self.autocal_retract_dist = config.getfloat("autocal_retract_dist", 2) 79 | self.autocal_retract_speed = config.getfloat("autocal_retract_speed", 10) 80 | self.autocal_sample_count = config.getfloat("autocal_sample_count", 3) 81 | self.autocal_tolerance = config.getfloat("autocal_tolerance", 0.008) 82 | self.autocal_max_retries = config.getfloat("autocal_max_retries", 3) 83 | 84 | self.contact_latency_min = config.getint("contact_latency_min", 0) 85 | self.contact_sensitivity = config.getint("contact_sensitivity", 0) 86 | 87 | self.trsync_timeout = config.getfloat( 88 | "trsync_timeout", TRSYNC_TIMEOUT_DEFAULT, maxval=0.25 89 | ) 90 | 91 | self.skip_firmware_version_check = config.getboolean( 92 | "skip_firmware_version_check", False 93 | ) 94 | 95 | # Load models 96 | self.model = None 97 | self.models = {} 98 | self.model_temp_builder = BeaconTempModelBuilder.load(config) 99 | self.model_temp = None 100 | self.fmin = None 101 | self.default_model_name = config.get("default_model_name", "default") 102 | self.model_manager = ModelManager(self) 103 | 104 | # Temperature sensor integration 105 | self.last_temp = 0 106 | self.last_mcu_temp = None 107 | self.measured_min = 99999999.0 108 | self.measured_max = 0.0 109 | self.mcu_temp = None 110 | self.thermistor = None 111 | 112 | self.last_sample = None 113 | self.last_received_sample = None 114 | self.last_z_result = 0 115 | self.last_probe_position = (0, 0) 116 | self.last_probe_result = None 117 | self.last_offset_result = None 118 | self.last_poke_result = None 119 | self.last_contact_msg = None 120 | self.hardware_failure = None 121 | 122 | self.mesh_helper = BeaconMeshHelper.create(self, config) 123 | self.homing_helper = BeaconHomingHelper.create(self, config) 124 | self.accel_helper = None 125 | self.accel_config = BeaconAccelConfig(config) 126 | 127 | self._stream_en = 0 128 | self._stream_timeout_timer = self.reactor.register_timer(self._stream_timeout) 129 | self._stream_callbacks = {} 130 | self._stream_latency_requests = {} 131 | self._stream_buffer = [] 132 | self._stream_buffer_count = 0 133 | self._stream_buffer_limit = STREAM_BUFFER_LIMIT_DEFAULT 134 | self._stream_buffer_limit_new = self._stream_buffer_limit 135 | self._stream_samples_queue = queue.Queue() 136 | self._stream_flush_event = threading.Event() 137 | self._log_stream = None 138 | self._data_filter = AlphaBetaFilter( 139 | config.getfloat("filter_alpha", 0.5), 140 | config.getfloat("filter_beta", 0.000001), 141 | ) 142 | self.trapq = None 143 | self.mod_axis_twist_comp = None 144 | self.get_z_compensation_value = lambda pos: 0.0 145 | 146 | mainsync = printer.lookup_object("mcu")._clocksync 147 | self._mcu = MCU(config, SecondarySync(self.reactor, mainsync)) 148 | orig_stats = self._mcu.stats 149 | 150 | def beacon_mcu_stats(eventtime): 151 | show, value = orig_stats(eventtime) 152 | value += " " + self._extend_stats() 153 | return show, value 154 | 155 | self._mcu.stats = beacon_mcu_stats 156 | printer.add_object("mcu " + self.name, self._mcu) 157 | self.cmd_queue = self._mcu.alloc_command_queue() 158 | self._endstop_shared = BeaconEndstopShared(self) 159 | self.mcu_probe = BeaconEndstopWrapper(self) 160 | self.mcu_contact_probe = BeaconContactEndstopWrapper(self, config) 161 | self._current_probe = "proximity" 162 | 163 | self.beacon_stream_cmd = None 164 | self.beacon_set_threshold = None 165 | self.beacon_home_cmd = None 166 | self.beacon_stop_home_cmd = None 167 | self.beacon_nvm_read_cmd = None 168 | self.beacon_contact_home_cmd = None 169 | self.beacon_contact_query_cmd = None 170 | self.beacon_contact_stop_home_cmd = None 171 | self.beacon_contact_set_latency_min_cmd = None 172 | self.beacon_contact_set_sensitivity_cmd = None 173 | 174 | # Register z_virtual_endstop 175 | register_as_probe = config.getboolean( 176 | "register_as_probe", sensor_id.is_unnamed() 177 | ) 178 | if register_as_probe: 179 | printer.lookup_object("pins").register_chip("probe", self) 180 | 181 | # Register event handlers 182 | printer.register_event_handler("klippy:connect", self._handle_connect) 183 | printer.register_event_handler("klippy:shutdown", self.force_stop_streaming) 184 | self._mcu.register_config_callback(self._build_config) 185 | self._mcu.register_response(self._handle_beacon_data, "beacon_data") 186 | self._mcu.register_response(self._handle_beacon_status, "beacon_status") 187 | self._mcu.register_response(self._handle_beacon_contact, "beacon_contact") 188 | 189 | # Register webhooks 190 | self._api_dump = APIDumpHelper( 191 | printer, 192 | lambda: self.streaming_session(self._api_dump_callback, latency=50), 193 | lambda stream: stream.stop(), 194 | None, 195 | ) 196 | sensor_id.register_endpoint("beacon/status", self._handle_req_status) 197 | sensor_id.register_endpoint("beacon/dump", self._handle_req_dump) 198 | 199 | # Register gcode commands 200 | sensor_id.register_command( 201 | "BEACON_STREAM", self.cmd_BEACON_STREAM, desc=self.cmd_BEACON_STREAM_help 202 | ) 203 | sensor_id.register_command( 204 | "BEACON_QUERY", self.cmd_BEACON_QUERY, desc=self.cmd_BEACON_QUERY_help 205 | ) 206 | sensor_id.register_command( 207 | "BEACON_CALIBRATE", 208 | self.cmd_BEACON_CALIBRATE, 209 | desc=self.cmd_BEACON_CALIBRATE_help, 210 | ) 211 | sensor_id.register_command( 212 | "BEACON_ESTIMATE_BACKLASH", 213 | self.cmd_BEACON_ESTIMATE_BACKLASH, 214 | desc=self.cmd_BEACON_ESTIMATE_BACKLASH_help, 215 | ) 216 | sensor_id.register_command("PROBE", self.cmd_PROBE, desc=self.cmd_PROBE_help) 217 | sensor_id.register_command( 218 | "PROBE_ACCURACY", self.cmd_PROBE_ACCURACY, desc=self.cmd_PROBE_ACCURACY_help 219 | ) 220 | sensor_id.register_command( 221 | "Z_OFFSET_APPLY_PROBE", 222 | self.cmd_Z_OFFSET_APPLY_PROBE, 223 | desc=self.cmd_Z_OFFSET_APPLY_PROBE_help, 224 | ) 225 | sensor_id.register_command( 226 | "BEACON_POKE", self.cmd_BEACON_POKE, desc=self.cmd_BEACON_POKE_help 227 | ) 228 | sensor_id.register_command( 229 | "BEACON_AUTO_CALIBRATE", 230 | self.cmd_BEACON_AUTO_CALIBRATE, 231 | desc=self.cmd_BEACON_AUTO_CALIBRATE_help, 232 | ) 233 | sensor_id.register_command( 234 | "BEACON_OFFSET_COMPARE", 235 | self.cmd_BEACON_OFFSET_COMPARE, 236 | desc=self.cmd_BEACON_OFFSET_COMPARE_help, 237 | ) 238 | if sensor_id.is_unnamed(): 239 | self._hook_probing_gcode(config, "z_tilt", "Z_TILT_ADJUST") 240 | self._hook_probing_gcode(config, "quad_gantry_level", "QUAD_GANTRY_LEVEL") 241 | self._hook_probing_gcode(config, "screws_tilt_adjust", "SCREWS_TILT_ADJUST") 242 | self._hook_probing_gcode(config, "delta_calibrate", "DELTA_CALIBRATE") 243 | 244 | # Event handlers 245 | 246 | def _handle_connect(self): 247 | self.phoming = self.printer.lookup_object("homing") 248 | self.mod_axis_twist_comp = self.printer.lookup_object( 249 | "axis_twist_compensation", None 250 | ) 251 | if self.mod_axis_twist_comp: 252 | if hasattr(self.mod_axis_twist_comp, "get_z_compensation_value"): 253 | self.get_z_compensation_value = ( 254 | lambda pos: self.mod_axis_twist_comp.get_z_compensation_value(pos) 255 | ) 256 | else: 257 | 258 | def _update_compensation(pos): 259 | cpos = list(pos) 260 | self.mod_axis_twist_comp._update_z_compensation_value(cpos) 261 | return cpos[2] - pos[2] 262 | 263 | self.get_z_compensation_value = _update_compensation 264 | 265 | if self.model is None: 266 | self.model = self.models.get(self.default_model_name, None) 267 | 268 | def _check_mcu_version(self): 269 | if self.skip_firmware_version_check: 270 | return "" 271 | updater = os.path.join(self.id.tracker.home_dir(), "update_firmware.py") 272 | if not os.path.exists(updater): 273 | logging.info( 274 | "Could not find Beacon firmware update script, won't check for update." 275 | ) 276 | return "" 277 | serialport = self.compat_serial_port(self._mcu) 278 | 279 | parent_conn, child_conn = multiprocessing.Pipe() 280 | 281 | def do(): 282 | try: 283 | output = subprocess.check_output( 284 | [updater, "check", serialport], universal_newlines=True 285 | ) 286 | child_conn.send((False, output.strip())) 287 | except Exception: 288 | child_conn.send((True, traceback.format_exc())) 289 | child_conn.close() 290 | 291 | child = multiprocessing.Process(target=do) 292 | child.daemon = True 293 | child.start() 294 | eventtime = self.reactor.monotonic() 295 | while child.is_alive(): 296 | eventtime = self.reactor.pause(eventtime + 0.1) 297 | (is_err, result) = parent_conn.recv() 298 | child.join() 299 | parent_conn.close() 300 | if is_err: 301 | logging.info("Executing Beacon update script failed: %s", result) 302 | elif result != "": 303 | self.gcode.respond_raw("!! " + result + "\n") 304 | pconfig = self.printer.lookup_object("configfile") 305 | try: 306 | pconfig.runtime_warning(result) 307 | except AttributeError: 308 | logging.info(result) 309 | return result 310 | return "" 311 | 312 | def _build_config(self): 313 | version_info = self._check_mcu_version() 314 | 315 | try: 316 | self.beacon_stream_cmd = self._mcu.lookup_command( 317 | "beacon_stream en=%u", cq=self.cmd_queue 318 | ) 319 | self.beacon_set_threshold = self._mcu.lookup_command( 320 | "beacon_set_threshold trigger=%u untrigger=%u", cq=self.cmd_queue 321 | ) 322 | self.beacon_home_cmd = self._mcu.lookup_command( 323 | "beacon_home trsync_oid=%c trigger_reason=%c trigger_invert=%c", 324 | cq=self.cmd_queue, 325 | ) 326 | self.beacon_stop_home_cmd = self._mcu.lookup_command( 327 | "beacon_stop_home", cq=self.cmd_queue 328 | ) 329 | self.beacon_nvm_read_cmd = self._mcu.lookup_query_command( 330 | "beacon_nvm_read len=%c offset=%hu", 331 | "beacon_nvm_data bytes=%*s offset=%hu", 332 | cq=self.cmd_queue, 333 | ) 334 | self.beacon_contact_home_cmd = self._mcu.lookup_command( 335 | "beacon_contact_home trsync_oid=%c trigger_reason=%c trigger_type=%c", 336 | cq=self.cmd_queue, 337 | ) 338 | self.beacon_contact_query_cmd = self._mcu.lookup_query_command( 339 | "beacon_contact_query", 340 | "beacon_contact_state triggered=%c detect_clock=%u", 341 | cq=self.cmd_queue, 342 | ) 343 | self.beacon_contact_stop_home_cmd = self._mcu.lookup_command( 344 | "beacon_contact_stop_home", 345 | cq=self.cmd_queue, 346 | ) 347 | try: 348 | self.beacon_contact_set_latency_min_cmd = self._mcu.lookup_command( 349 | "beacon_contact_set_latency_min latency_min=%c", 350 | cq=self.cmd_queue, 351 | ) 352 | except msgproto.error: 353 | pass 354 | try: 355 | self.beacon_contact_set_sensitivity_cmd = self._mcu.lookup_command( 356 | "beacon_contact_set_sensitivity sensitivity=%c", 357 | cq=self.cmd_queue, 358 | ) 359 | except msgproto.error: 360 | pass 361 | 362 | constants = self._mcu.get_constants() 363 | 364 | self._mcu_freq = self._mcu.get_constant_float("CLOCK_FREQ") 365 | 366 | self.inv_adc_max = 1.0 / constants.get("ADC_MAX") 367 | self.temp_smooth_count = constants.get("BEACON_ADC_SMOOTH_COUNT") 368 | self.thermistor = thermistor.Thermistor(10000.0, 0.0) 369 | self.thermistor.setup_coefficients_beta(25.0, 47000.0, 4101.0) 370 | 371 | self.toolhead = self.printer.lookup_object("toolhead") 372 | self.kinematics = self.toolhead.get_kinematics() 373 | self.trapq = self.toolhead.get_trapq() 374 | 375 | self.mcu_temp = BeaconMCUTempHelper.build_with_nvm(self) 376 | self.model_temp = self.model_temp_builder.build_with_nvm(self) 377 | if self.model_temp: 378 | self.fmin = self.model_temp.fmin 379 | if self.model is None: 380 | self.model = self.models.get(self.default_model_name, None) 381 | if self.model: 382 | self._apply_threshold() 383 | 384 | if self.beacon_stream_cmd is not None: 385 | self.beacon_stream_cmd.send([1 if self._stream_en else 0]) 386 | if self._stream_en: 387 | curtime = self.reactor.monotonic() 388 | self.reactor.update_timer( 389 | self._stream_timeout_timer, curtime + STREAM_TIMEOUT 390 | ) 391 | else: 392 | self.reactor.update_timer( 393 | self._stream_timeout_timer, self.reactor.NEVER 394 | ) 395 | 396 | if constants.get("BEACON_HAS_ACCEL", 0) == 1: 397 | logging.info("Enabling Beacon accelerometer") 398 | if self.accel_helper is None: 399 | self.accel_helper = BeaconAccelHelper( 400 | self, self.accel_config, constants 401 | ) 402 | else: 403 | self.accel_helper.reinit(constants) 404 | 405 | except msgproto.error as e: 406 | if version_info != "": 407 | raise msgproto.error(version_info + "\n\n" + str(e)) 408 | raise 409 | 410 | def _extend_stats(self): 411 | parts = [ 412 | "coil_temp=%.1f" % (self.last_temp,), 413 | "refs=%d" % (self._stream_en,), 414 | ] 415 | if self.last_mcu_temp is not None: 416 | (mcu_temp, supply_voltage) = self.last_mcu_temp 417 | parts.append("mcu_temp=%.2f" % (mcu_temp,)) 418 | parts.append("supply_voltage=%.3f" % (supply_voltage,)) 419 | 420 | return " ".join(parts) 421 | 422 | def _api_dump_callback(self, sample): 423 | tmp = [sample.get(key, None) for key in API_DUMP_FIELDS] 424 | self._api_dump.buffer.append(tmp) 425 | 426 | # Virtual endstop 427 | 428 | def setup_pin(self, pin_type, pin_params): 429 | if pin_type != "endstop" or pin_params["pin"] != "z_virtual_endstop": 430 | raise pins.error("Probe virtual endstop only useful as endstop pin") 431 | if pin_params["invert"] or pin_params["pullup"]: 432 | raise pins.error("Can not pullup/invert probe virtual endstop") 433 | return self.mcu_probe 434 | 435 | # Probe interface 436 | 437 | def multi_probe_begin(self): 438 | self._start_streaming() 439 | 440 | def multi_probe_end(self): 441 | self._stop_streaming() 442 | 443 | def get_offsets(self): 444 | if self._current_probe == "contact": 445 | return 0, 0, 0 446 | else: 447 | return self.x_offset, self.y_offset, self.trigger_distance 448 | 449 | def get_lift_speed(self, gcmd=None): 450 | if gcmd is not None: 451 | return gcmd.get_float("LIFT_SPEED", self.lift_speed, above=0.0) 452 | return self.lift_speed 453 | 454 | def run_probe(self, gcmd): 455 | method = gcmd.get("PROBE_METHOD", self.default_probe_method).lower() 456 | self._current_probe = method 457 | if method == "proximity": 458 | return self._run_probe_proximity(gcmd) 459 | elif method == "contact": 460 | self._start_streaming() 461 | try: 462 | return self._run_probe_contact(gcmd) 463 | finally: 464 | self._stop_streaming() 465 | else: 466 | raise gcmd.error("Invalid PROBE_METHOD, valid choices: proximity, contact") 467 | 468 | def _move_to_probing_height(self, speed): 469 | target = self.trigger_distance 470 | top = target + self.backlash_comp 471 | cur_z = self.toolhead.get_position()[2] 472 | if cur_z < top: 473 | self.toolhead.manual_move([None, None, top], speed) 474 | self.toolhead.manual_move([None, None, target], speed) 475 | self.toolhead.wait_moves() 476 | 477 | def _run_probe_proximity(self, gcmd): 478 | if self.model is None: 479 | raise self.printer.command_error("No Beacon model loaded") 480 | 481 | speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.0) 482 | allow_faulty = gcmd.get_int("ALLOW_FAULTY_COORDINATE", 0) != 0 483 | toolhead = self.printer.lookup_object("toolhead") 484 | curtime = self.reactor.monotonic() 485 | if "z" not in toolhead.get_status(curtime)["homed_axes"]: 486 | raise self.printer.command_error("Must home before probe") 487 | 488 | self._start_streaming() 489 | try: 490 | return self._probe(speed, allow_faulty=allow_faulty) 491 | finally: 492 | self._stop_streaming() 493 | 494 | def _probing_move_to_probing_height(self, speed): 495 | curtime = self.reactor.monotonic() 496 | status = self.kinematics.get_status(curtime) 497 | pos = self.toolhead.get_position() 498 | pos[2] = status["axis_minimum"][2] 499 | try: 500 | self.phoming.probing_move(self.mcu_probe, pos, speed) 501 | except self.printer.command_error as e: 502 | reason = str(e) 503 | if "Timeout during probing move" in reason: 504 | reason += probe.HINT_TIMEOUT 505 | raise self.printer.command_error(reason) 506 | 507 | def _probe(self, speed, num_samples=10, allow_faulty=False): 508 | target = self.trigger_distance 509 | tdt = self.trigger_dive_threshold 510 | (dist, samples) = self._sample(5, num_samples) 511 | 512 | x, y = samples[0]["pos"][0:2] 513 | if self._is_faulty_coordinate(x, y, True): 514 | msg = "Probing within a faulty area" 515 | if not allow_faulty: 516 | raise self.printer.command_error(msg) 517 | else: 518 | self.gcode.respond_raw("!! " + msg + "\n") 519 | 520 | if dist > target + tdt: 521 | # If we are above the dive threshold right now, we'll need to 522 | # do probing move and then re-measure 523 | self._probing_move_to_probing_height(speed) 524 | (dist, samples) = self._sample(self.z_settling_time, num_samples) 525 | elif math.isinf(dist) and dist < 0: 526 | # We were below the valid range of the model 527 | msg = "Attempted to probe with Beacon below calibrated model range" 528 | raise self.printer.command_error(msg) 529 | elif self.toolhead.get_position()[2] < target - tdt: 530 | # We are below the probing target height, we'll move to the 531 | # correct height and take a new sample. 532 | self._move_to_probing_height(speed) 533 | (dist, samples) = self._sample(self.z_settling_time, num_samples) 534 | 535 | pos = samples[0]["pos"] 536 | 537 | self.gcode.respond_info( 538 | "probe at %.3f,%.3f,%.3f is z=%.6f" % (pos[0], pos[1], pos[2], dist) 539 | ) 540 | 541 | return [pos[0], pos[1], pos[2] + target - dist] 542 | 543 | def _run_probe_contact(self, gcmd): 544 | self.toolhead.wait_moves() 545 | speed = gcmd.get_float( 546 | "PROBE_SPEED", self.autocal_speed, above=0.0, maxval=self.autocal_max_speed 547 | ) 548 | lift_speed = self.get_lift_speed(gcmd) 549 | sample_count = gcmd.get_int("SAMPLES", self.autocal_sample_count, minval=1) 550 | retract_dist = gcmd.get_float( 551 | "SAMPLE_RETRACT_DIST", self.autocal_retract_dist, minval=1 552 | ) 553 | tolerance = gcmd.get_float( 554 | "SAMPLES_TOLERANCE", self.autocal_tolerance, above=0.0 555 | ) 556 | max_retries = gcmd.get_int( 557 | "SAMPLES_TOLERANCE_RETRIES", self.autocal_max_retries, minval=0 558 | ) 559 | samples_result = gcmd.get("SAMPLES_RESULT", "mean") 560 | drop_n = gcmd.get_int("SAMPLES_DROP", 0, minval=0) 561 | retries = 0 562 | samples = [] 563 | 564 | posxy = self.toolhead.get_position()[:2] 565 | 566 | self.mcu_contact_probe.activate_gcode.run_gcode_from_command() 567 | try: 568 | while len(samples) < sample_count: 569 | pos = self._probe_contact(speed) 570 | self.toolhead.manual_move(posxy + [pos[2] + retract_dist], lift_speed) 571 | if drop_n > 0: 572 | drop_n -= 1 573 | continue 574 | samples.append(pos[2]) 575 | spread = max(samples) - min(samples) 576 | if spread > tolerance: 577 | if retries >= max_retries: 578 | raise gcmd.error("Probe samples exceed sample_tolerance") 579 | gcmd.respond_info("Probe samples exceed tolerance. Retrying...") 580 | samples = [] 581 | retries += 1 582 | if samples_result == "median": 583 | return posxy + [median(samples)] 584 | else: 585 | return posxy + [float(np.mean(samples))] 586 | finally: 587 | self.mcu_contact_probe.deactivate_gcode.run_gcode_from_command() 588 | 589 | def _probe_contact(self, speed): 590 | self.toolhead.get_last_move_time() 591 | self._sample_async() 592 | start_pos = self.toolhead.get_position() 593 | hmove = HomingMove(self.printer, [(self.mcu_contact_probe, "contact")]) 594 | pos = start_pos[:] 595 | pos[2] = -2 596 | try: 597 | epos = hmove.homing_move(pos, speed, probe_pos=True)[:3] 598 | except self.printer.command_error as e: 599 | if self.printer.is_shutdown(): 600 | reason = "Probing failed due to printer shutdown" 601 | else: 602 | reason = str(e) 603 | if "Timeout during probing move" in reason: 604 | reason += probe.HINT_TIMEOUT 605 | raise self.printer.command_error(reason) 606 | epos[2] += self.get_z_compensation_value(pos) 607 | self.gcode.respond_info( 608 | "probe at %.3f,%.3f is z=%.6f" % (epos[0], epos[1], epos[2]) 609 | ) 610 | return epos[:3] 611 | 612 | # Accelerometer interface 613 | 614 | def start_internal_client(self): 615 | if not self.accel_helper: 616 | msg = "This Beacon has no accelerometer" 617 | raise self.printer.command_error(msg) 618 | return self.accel_helper.start_internal_client() 619 | 620 | # Calibration routines 621 | 622 | def _start_calibration(self, gcmd): 623 | allow_faulty = gcmd.get_int("ALLOW_FAULTY_COORDINATE", 0) != 0 624 | nozzle_z = gcmd.get_float("NOZZLE_Z", self.cal_nozzle_z) 625 | if gcmd.get("SKIP_MANUAL_PROBE", None) is not None: 626 | kin = self.kinematics 627 | kin_spos = { 628 | s.get_name(): s.get_commanded_position() for s in kin.get_steppers() 629 | } 630 | kin_pos = kin.calc_position(kin_spos) 631 | if self._is_faulty_coordinate(kin_pos[0], kin_pos[1]): 632 | msg = "Calibrating within a faulty area" 633 | if not allow_faulty: 634 | raise gcmd.error(msg) 635 | else: 636 | gcmd.respond_raw("!! " + msg + "\n") 637 | self._calibrate(gcmd, kin_pos, nozzle_z, False) 638 | else: 639 | curtime = self.printer.get_reactor().monotonic() 640 | kin_status = self.toolhead.get_status(curtime) 641 | if "xy" not in kin_status["homed_axes"]: 642 | raise self.printer.command_error("Must home X and Y before calibration") 643 | 644 | kin_pos = self.toolhead.get_position() 645 | if self._is_faulty_coordinate(kin_pos[0], kin_pos[1]): 646 | msg = "Calibrating within a faulty area" 647 | if not allow_faulty: 648 | raise gcmd.error(msg) 649 | else: 650 | gcmd.respond_raw("!! " + msg + "\n") 651 | 652 | forced_z = False 653 | if "z" not in kin_status["homed_axes"]: 654 | self.toolhead.get_last_move_time() 655 | pos = self.toolhead.get_position() 656 | pos[2] = ( 657 | kin_status["axis_maximum"][2] 658 | - 2.0 659 | - gcmd.get_float("CEIL", self.cal_ceil) 660 | ) 661 | self.compat_toolhead_set_position_homing_z(self.toolhead, pos) 662 | forced_z = True 663 | 664 | def cb(kin_pos): 665 | return self._calibrate(gcmd, kin_pos, nozzle_z, forced_z) 666 | 667 | manual_probe.ManualProbeHelper(self.printer, gcmd, cb) 668 | 669 | def _calibrate(self, gcmd, kin_pos, cal_nozzle_z, forced_z, is_auto=False): 670 | if kin_pos is None: 671 | if forced_z: 672 | kin = self.kinematics 673 | self.compat_kin_note_z_not_homed(kin) 674 | return 675 | 676 | gcmd.respond_info("Beacon calibration starting") 677 | cal_floor = gcmd.get_float("FLOOR", self.cal_floor) 678 | cal_ceil = gcmd.get_float("CEIL", self.cal_ceil) 679 | cal_speed = gcmd.get_float("DESCEND_SPEED", self.cal_speed) 680 | move_speed = gcmd.get_float("MOVE_SPEED", self.cal_move_speed) 681 | model_name = gcmd.get("MODEL_NAME", "default") 682 | 683 | toolhead = self.toolhead 684 | toolhead.wait_moves() 685 | 686 | # Move coordinate system to nozzle location 687 | self.toolhead.get_last_move_time() 688 | curpos = toolhead.get_position() 689 | curpos[2] = cal_nozzle_z 690 | toolhead.set_position(curpos) 691 | 692 | # Move over to probe coordinate and pull out backlash 693 | curpos[2] = cal_ceil + self.backlash_comp 694 | toolhead.manual_move(curpos, move_speed) # Up 695 | curpos[0] -= self.x_offset 696 | curpos[1] -= self.y_offset 697 | toolhead.manual_move(curpos, move_speed) # Over 698 | curpos[2] = cal_ceil 699 | toolhead.manual_move(curpos, move_speed) # Down 700 | toolhead.wait_moves() 701 | 702 | samples = [] 703 | 704 | def cb(sample): 705 | samples.append(sample) 706 | 707 | # Descend while sampling 708 | toolhead.flush_step_generation() 709 | try: 710 | self._start_streaming() 711 | self._sample_printtime_sync(50) 712 | with self.streaming_session(cb): 713 | self._sample_printtime_sync(50) 714 | toolhead.dwell(0.250) 715 | curpos[2] = cal_floor 716 | toolhead.manual_move(curpos, cal_speed) 717 | toolhead.flush_step_generation() 718 | self._sample_printtime_sync(50) 719 | finally: 720 | self._stop_streaming() 721 | 722 | # Fit the sampled data 723 | z_offset = [s["pos"][2] for s in samples] 724 | freq = [s["freq"] for s in samples] 725 | temp = [s["temp"] for s in samples] 726 | inv_freq = [1 / f for f in freq] 727 | poly = Polynomial.fit(inv_freq, z_offset, 9) 728 | temp_median = median(temp) 729 | self.model = BeaconModel( 730 | model_name, self, poly, temp_median, min(z_offset), max(z_offset) 731 | ) 732 | self.models[self.model.name] = self.model 733 | self.model.save(self, not is_auto) 734 | self._apply_threshold() 735 | 736 | # Dump calibration curve 737 | fn = "/tmp/beacon-calibrate-" + time.strftime("%Y%m%d_%H%M%S") + ".csv" 738 | with open(fn, "w") as f: 739 | f.write("freq,z,temp\n") 740 | for i in range(len(freq)): 741 | f.write("%.5f,%.5f,%.3f\n" % (freq[i], z_offset[i], temp[i])) 742 | 743 | gcmd.respond_info( 744 | "Beacon calibrated at %.3f,%.3f from " 745 | "%.3f to %.3f, speed %.2f mm/s, temp %.2fC" 746 | % (curpos[0], curpos[1], cal_floor, cal_ceil, cal_speed, temp_median) 747 | ) 748 | 749 | # Internal 750 | 751 | def _update_thresholds(self, moving_up=False): 752 | self.trigger_freq = self.dist_to_freq(self.trigger_distance, self.last_temp) 753 | self.untrigger_freq = self.trigger_freq * (1 - self.trigger_hysteresis) 754 | 755 | def _apply_threshold(self, moving_up=False): 756 | self._update_thresholds() 757 | trigger_c = int(self.freq_to_count(self.trigger_freq)) 758 | untrigger_c = int(self.freq_to_count(self.untrigger_freq)) 759 | if self.beacon_set_threshold is not None: 760 | self.beacon_set_threshold.send([trigger_c, untrigger_c]) 761 | 762 | def _register_model(self, name, model): 763 | if name in self.models: 764 | raise self.printer.config_error( 765 | "Multiple Beacon models with samename '%s'" % (name,) 766 | ) 767 | self.models[name] = model 768 | 769 | def _is_faulty_coordinate(self, x, y, add_offsets=False): 770 | if not self.mesh_helper: 771 | return False 772 | return self.mesh_helper._is_faulty_coordinate(x, y, add_offsets) 773 | 774 | def _handle_beacon_status(self, params): 775 | if self.mcu_temp is not None: 776 | self.last_mcu_temp = self.mcu_temp.compensate( 777 | self, params["mcu_temp"], params["supply_voltage"] 778 | ) 779 | if self.thermistor is not None: 780 | self.last_temp = self.thermistor.calc_temp( 781 | params["coil_temp"] / self.temp_smooth_count * self.inv_adc_max 782 | ) 783 | 784 | def _handle_beacon_contact(self, params): 785 | self.last_contact_msg = params 786 | 787 | def _hook_probing_gcode(self, config, module, cmd): 788 | if not config.has_section(module): 789 | return 790 | section = config.getsection(module) 791 | mod = self.printer.load_object(section, module) 792 | if mod is None: 793 | return 794 | orig = self.gcode.register_command(cmd, None) 795 | 796 | def cb(gcmd): 797 | self._current_probe = gcmd.get( 798 | "PROBE_METHOD", self.default_probe_method 799 | ).lower() 800 | return orig(gcmd) 801 | 802 | self.gcode.register_command(cmd, cb) 803 | 804 | # Streaming mode 805 | 806 | def _check_hardware(self, sample): 807 | if not self.hardware_failure: 808 | msg = None 809 | if sample["data"] == 0xFFFFFFF: 810 | msg = "coil is shorted or not connected" 811 | elif self.fmin is not None and sample["freq"] > 1.35 * self.fmin: 812 | msg = "coil expected max frequency exceeded" 813 | if msg: 814 | msg = "Beacon hardware issue: " + msg 815 | self.hardware_failure = msg 816 | logging.error(msg) 817 | if self._stream_en: 818 | self.printer.invoke_shutdown(msg) 819 | else: 820 | self.gcode.respond_raw("!! " + msg + "\n") 821 | elif self._stream_en: 822 | self.printer.invoke_shutdown(self.hardware_failure) 823 | 824 | def _clock32_to_time(self, clock): 825 | clock64 = self._mcu.clock32_to_clock64(clock) 826 | return self._mcu.clock_to_print_time(clock64) 827 | 828 | def _start_streaming(self): 829 | if self._stream_en == 0 and self.beacon_stream_cmd is not None: 830 | self.beacon_stream_cmd.send([1]) 831 | curtime = self.reactor.monotonic() 832 | self.reactor.update_timer( 833 | self._stream_timeout_timer, curtime + STREAM_TIMEOUT 834 | ) 835 | self._stream_en += 1 836 | self._data_filter.reset() 837 | self._stream_flush() 838 | 839 | def _stop_streaming(self): 840 | self._stream_en -= 1 841 | if self._stream_en == 0: 842 | self.reactor.update_timer(self._stream_timeout_timer, self.reactor.NEVER) 843 | if self.beacon_stream_cmd is not None: 844 | self.beacon_stream_cmd.send([0]) 845 | self._stream_flush() 846 | 847 | def force_stop_streaming(self): 848 | self.reactor.update_timer(self._stream_timeout_timer, self.reactor.NEVER) 849 | if self.beacon_stream_cmd is not None: 850 | self.beacon_stream_cmd.send([0]) 851 | self._stream_flush() 852 | 853 | def _stream_timeout(self, eventtime): 854 | if self._stream_flush(): 855 | return eventtime + STREAM_TIMEOUT 856 | if not self._stream_en: 857 | return self.reactor.NEVER 858 | if not self.printer.is_shutdown(): 859 | msg = "Beacon sensor not receiving data" 860 | logging.error(msg) 861 | self.printer.invoke_shutdown(msg) 862 | return self.reactor.NEVER 863 | 864 | def request_stream_latency(self, latency): 865 | next_key = 0 866 | if self._stream_latency_requests: 867 | next_key = max(self._stream_latency_requests.keys()) + 1 868 | new_limit = STREAM_BUFFER_LIMIT_DEFAULT 869 | self._stream_latency_requests[next_key] = latency 870 | min_requested = min(self._stream_latency_requests.values()) 871 | if min_requested < new_limit: 872 | new_limit = min_requested 873 | if new_limit < 1: 874 | new_limit = 1 875 | self._stream_buffer_limit_new = new_limit 876 | return next_key 877 | 878 | def drop_stream_latency_request(self, key): 879 | self._stream_latency_requests.pop(key, None) 880 | new_limit = STREAM_BUFFER_LIMIT_DEFAULT 881 | if self._stream_latency_requests: 882 | min_requested = min(self._stream_latency_requests.values()) 883 | if min_requested < new_limit: 884 | new_limit = min_requested 885 | if new_limit < 1: 886 | new_limit = 1 887 | self._stream_buffer_limit_new = new_limit 888 | 889 | def streaming_session(self, callback, completion_callback=None, latency=None): 890 | return StreamingHelper(self, callback, completion_callback, latency) 891 | 892 | def _stream_flush_message(self, msg): 893 | last = None 894 | for sample in msg: 895 | (clock, data) = sample 896 | temp = self.last_temp 897 | if self.model_temp is not None and not (-40 < temp < 180): 898 | msg = ( 899 | "Beacon temperature sensor faulty(read %.2f C)," 900 | " disabling temperature compensation" % (temp,) 901 | ) 902 | logging.error(msg) 903 | self.gcode.respond_raw("!! " + msg + "\n") 904 | self.model_temp = None 905 | if temp: 906 | self.measured_min = min(self.measured_min, temp) 907 | self.measured_max = max(self.measured_max, temp) 908 | 909 | clock = self._mcu.clock32_to_clock64(clock) 910 | time = self._mcu.clock_to_print_time(clock) 911 | self._data_filter.update(time, data) 912 | data_smooth = self._data_filter.value() 913 | freq = self.count_to_freq(data_smooth) 914 | dist = self.freq_to_dist(freq, temp) 915 | pos = self._get_position_at_time(time) 916 | if pos is not None: 917 | if dist is not None: 918 | dist -= self.get_z_compensation_value(pos) 919 | last = sample = { 920 | "temp": temp, 921 | "clock": clock, 922 | "time": time, 923 | "data": data, 924 | "data_smooth": data_smooth, 925 | "freq": freq, 926 | "dist": dist, 927 | } 928 | if pos is not None: 929 | sample["pos"] = pos 930 | self._check_hardware(sample) 931 | 932 | if len(self._stream_callbacks) > 0: 933 | for cb in list(self._stream_callbacks.values()): 934 | cb(sample) 935 | if last is not None: 936 | last = last.copy() 937 | dist = last["dist"] 938 | if dist is None or np.isinf(dist) or np.isnan(dist): 939 | del last["dist"] 940 | self.last_received_sample = last 941 | 942 | def _stream_flush(self): 943 | self._stream_flush_event.clear() 944 | updated_timer = False 945 | while True: 946 | try: 947 | samples = self._stream_samples_queue.get_nowait() 948 | updated_timer = False 949 | for sample in samples: 950 | if not updated_timer: 951 | curtime = self.reactor.monotonic() 952 | self.reactor.update_timer( 953 | self._stream_timeout_timer, curtime + STREAM_TIMEOUT 954 | ) 955 | updated_timer = True 956 | self._stream_flush_message(sample) 957 | except queue.Empty: 958 | return updated_timer 959 | 960 | def _stream_flush_schedule(self): 961 | force = self._stream_en == 0 # When streaming is disabled, let all through 962 | if self._stream_buffer_limit_new != self._stream_buffer_limit: 963 | force = True 964 | self._stream_buffer_limit = self._stream_buffer_limit_new 965 | if not force and self._stream_buffer_count < self._stream_buffer_limit: 966 | return 967 | self._stream_samples_queue.put_nowait(self._stream_buffer) 968 | self._stream_buffer = [] 969 | self._stream_buffer_count = 0 970 | if self._stream_flush_event.is_set(): 971 | return 972 | self._stream_flush_event.set() 973 | self.reactor.register_async_callback(lambda e: self._stream_flush()) 974 | 975 | def _handle_beacon_data(self, params): 976 | if self.trapq is None: 977 | return 978 | 979 | buf = bytearray(params["data"]) 980 | sample_count = params["samples"] 981 | start_clock = params["start_clock"] 982 | delta_clock = ( 983 | params["delta_clock"] / (sample_count - 1) if sample_count > 1 else 0 984 | ) 985 | 986 | samples = [] 987 | data = 0 988 | for i in range(0, sample_count): 989 | if buf[0] & 0x80 == 0: 990 | delta = ((buf[0] & 0x7F) << 8) + buf[1] 991 | data = data + delta - ((buf[0] & 0x40) << 9) 992 | buf = buf[2:] 993 | else: 994 | data = (buf[0] & 0x7F) << 24 | buf[1] << 16 | buf[2] << 8 | buf[3] 995 | buf = buf[4:] 996 | clock = start_clock + int(round(i * delta_clock)) 997 | samples.append((clock, data)) 998 | 999 | self._stream_buffer.append(samples) 1000 | self._stream_buffer_count += len(samples) 1001 | self._stream_flush_schedule() 1002 | 1003 | def _get_position_at_time(self, print_time): 1004 | kin = self.kinematics 1005 | pos = { 1006 | s.get_name(): s.mcu_to_commanded_position( 1007 | s.get_past_mcu_position(print_time) 1008 | ) 1009 | for s in kin.get_steppers() 1010 | } 1011 | return kin.calc_position(pos) 1012 | 1013 | def _sample_printtime_sync(self, skip=0, count=1): 1014 | move_time = self.toolhead.get_last_move_time() 1015 | settle_clock = self._mcu.print_time_to_clock(move_time) 1016 | samples = [] 1017 | total = skip + count 1018 | 1019 | def cb(sample): 1020 | if sample["clock"] >= settle_clock: 1021 | samples.append(sample) 1022 | if len(samples) >= total: 1023 | raise StopStreaming 1024 | 1025 | with self.streaming_session(cb, latency=skip + count) as ss: 1026 | ss.wait() 1027 | 1028 | samples = samples[skip:] 1029 | 1030 | if count == 1: 1031 | return samples[0] 1032 | else: 1033 | return samples 1034 | 1035 | def _sample(self, skip, count): 1036 | samples = self._sample_printtime_sync(skip, count) 1037 | return (median([s["dist"] for s in samples]), samples) 1038 | 1039 | def _sample_async(self, count=1): 1040 | samples = [] 1041 | 1042 | def cb(sample): 1043 | samples.append(sample) 1044 | if len(samples) >= count: 1045 | raise StopStreaming 1046 | 1047 | with self.streaming_session(cb, latency=count) as ss: 1048 | ss.wait() 1049 | 1050 | if count == 1: 1051 | return samples[0] 1052 | else: 1053 | return samples 1054 | 1055 | def count_to_freq(self, count): 1056 | return count * self._mcu_freq / (2**28) 1057 | 1058 | def freq_to_count(self, freq): 1059 | return freq * (2**28) / self._mcu_freq 1060 | 1061 | def dist_to_freq(self, dist, temp): 1062 | if self.model is None: 1063 | return None 1064 | return self.model.dist_to_freq(dist, temp) 1065 | 1066 | def freq_to_dist(self, freq, temp): 1067 | if self.model is None: 1068 | return None 1069 | return self.model.freq_to_dist(freq, temp) 1070 | 1071 | def get_status(self, eventtime): 1072 | model = None 1073 | if self.model is not None: 1074 | model = self.model.name 1075 | return { 1076 | "last_sample": self.last_sample, 1077 | "last_received_sample": self.last_received_sample, 1078 | "last_z_result": self.last_z_result, 1079 | "last_probe_position": self.last_probe_position, 1080 | "last_probe_result": self.last_probe_result, 1081 | "last_offset_result": self.last_offset_result, 1082 | "last_poke_result": self.last_poke_result, 1083 | "model": model, 1084 | } 1085 | 1086 | # Webhook handlers 1087 | 1088 | def _handle_req_status(self, web_request): 1089 | temp = None 1090 | sample = self._sample_async() 1091 | out = { 1092 | "freq": sample["freq"], 1093 | "dist": sample["dist"], 1094 | } 1095 | temp = sample["temp"] 1096 | if temp is not None: 1097 | out["temp"] = temp 1098 | web_request.send(out) 1099 | 1100 | def _handle_req_dump(self, web_request): 1101 | self._api_dump.add_web_client(web_request) 1102 | web_request.send({"header": API_DUMP_FIELDS}) 1103 | 1104 | # Compat wrappers 1105 | 1106 | def compat_toolhead_set_position_homing_z(self, toolhead, pos): 1107 | func = toolhead.set_position 1108 | kind = tuple 1109 | if hasattr(func, "__defaults__"): # Python 3 1110 | kind = type(func.__defaults__[0]) 1111 | else: # Python 2 1112 | kind = type(func.func_defaults[0]) 1113 | if kind is str: 1114 | return toolhead.set_position(pos, homing_axes="z") 1115 | else: 1116 | return toolhead.set_position(pos, homing_axes=[2]) 1117 | 1118 | def compat_kin_note_z_not_homed(self, kin): 1119 | if hasattr(kin, "note_z_not_homed"): 1120 | kin.note_z_not_homed() 1121 | elif hasattr(kin, "clear_homing_state"): 1122 | kin.clear_homing_state("z") 1123 | 1124 | def compat_serial_port(self, mcu): 1125 | if hasattr(mcu, "_serialport"): 1126 | return mcu._serialport 1127 | elif hasattr(mcu, "_conn_helper"): 1128 | return mcu._conn_helper.get_serialport()[0] 1129 | else: 1130 | raise Exception("Could not determine serial port") 1131 | 1132 | # GCode command handlers 1133 | 1134 | cmd_PROBE_help = "Probe Z-height at current XY position" 1135 | 1136 | def cmd_PROBE(self, gcmd): 1137 | self.last_probe_result = "failed" 1138 | pos = self.run_probe(gcmd) 1139 | gcmd.respond_info("Result is z=%.6f" % (pos[2],)) 1140 | offset = self.get_offsets() 1141 | self.last_z_result = pos[2] - offset[2] 1142 | self.last_probe_position = (pos[0] - offset[0], pos[1] - offset[1]) 1143 | self.last_probe_result = "ok" 1144 | 1145 | cmd_BEACON_CALIBRATE_help = "Calibrate beacon response curve" 1146 | 1147 | def cmd_BEACON_CALIBRATE(self, gcmd): 1148 | self._start_calibration(gcmd) 1149 | 1150 | cmd_BEACON_ESTIMATE_BACKLASH_help = "Estimate Z axis backlash" 1151 | 1152 | def cmd_BEACON_ESTIMATE_BACKLASH(self, gcmd): 1153 | # Get to correct Z height 1154 | overrun = gcmd.get_float("OVERRUN", 1.0) 1155 | speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.0) 1156 | cur_z = self.toolhead.get_position()[2] 1157 | self.toolhead.manual_move([None, None, cur_z + overrun], speed) 1158 | self.run_probe(gcmd) 1159 | 1160 | lift_speed = self.get_lift_speed(gcmd) 1161 | target = gcmd.get_float("Z", self.trigger_distance) 1162 | 1163 | num_samples = gcmd.get_int("SAMPLES", 20) 1164 | wait = self.z_settling_time 1165 | 1166 | samples_up = [] 1167 | samples_down = [] 1168 | 1169 | next_dir = -1 1170 | 1171 | try: 1172 | self._start_streaming() 1173 | 1174 | (cur_dist, _samples) = self._sample(wait, 10) 1175 | pos = self.toolhead.get_position() 1176 | missing = target - cur_dist 1177 | target = pos[2] + missing 1178 | gcmd.respond_info("Target kinematic Z is %.3f" % (target,)) 1179 | 1180 | if target - overrun < 0: 1181 | raise gcmd.error("Target minus overrun must exceed 0mm") 1182 | 1183 | while len(samples_up) + len(samples_down) < num_samples: 1184 | liftpos = [None, None, target + overrun * next_dir] 1185 | self.toolhead.manual_move(liftpos, lift_speed) 1186 | liftpos = [None, None, target] 1187 | self.toolhead.manual_move(liftpos, lift_speed) 1188 | self.toolhead.wait_moves() 1189 | (dist, _samples) = self._sample(wait, 10) 1190 | {-1: samples_up, 1: samples_down}[next_dir].append(dist) 1191 | next_dir = next_dir * -1 1192 | 1193 | finally: 1194 | self._stop_streaming() 1195 | 1196 | res_up = median(samples_up) 1197 | res_down = median(samples_down) 1198 | 1199 | gcmd.respond_info( 1200 | "Median distance moving up %.5f, down %.5f, " 1201 | "delta %.5f over %d samples" 1202 | % (res_up, res_down, res_down - res_up, num_samples) 1203 | ) 1204 | 1205 | cmd_BEACON_QUERY_help = "Take a sample from the sensor" 1206 | 1207 | def cmd_BEACON_QUERY(self, gcmd): 1208 | sample = self._sample_async() 1209 | last_value = sample["freq"] 1210 | dist = sample["dist"] 1211 | temp = sample["temp"] 1212 | self.last_sample = { 1213 | "time": sample["time"], 1214 | "value": last_value, 1215 | "temp": temp, 1216 | "dist": None if dist is None or np.isinf(dist) or np.isnan(dist) else dist, 1217 | } 1218 | if dist is None: 1219 | gcmd.respond_info( 1220 | "Last reading: %.2fHz, %.2fC, no model" 1221 | % ( 1222 | last_value, 1223 | temp, 1224 | ) 1225 | ) 1226 | else: 1227 | gcmd.respond_info( 1228 | "Last reading: %.2fHz, %.2fC, %.5fmm" % (last_value, temp, dist) 1229 | ) 1230 | 1231 | cmd_BEACON_STREAM_help = "Enable Beacon Streaming" 1232 | 1233 | def cmd_BEACON_STREAM(self, gcmd): 1234 | if self._log_stream is not None: 1235 | self._log_stream.stop() 1236 | self._log_stream = None 1237 | gcmd.respond_info("Beacon Streaming disabled") 1238 | else: 1239 | f = None 1240 | completion_cb = None 1241 | fn = gcmd.get("FILENAME") 1242 | f = open(fn, "w") 1243 | 1244 | def close_file(): 1245 | f.close() 1246 | 1247 | completion_cb = close_file 1248 | f.write("time,data,data_smooth,freq,dist,temp,pos_x,pos_y,pos_z\n") 1249 | 1250 | def cb(sample): 1251 | pos = sample.get("pos", None) 1252 | obj = "%.4f,%d,%.2f,%.5f,%.5f,%.2f,%s,%s,%s\n" % ( 1253 | sample["time"], 1254 | sample["data"], 1255 | sample["data_smooth"], 1256 | sample["freq"], 1257 | sample["dist"], 1258 | sample["temp"], 1259 | "%.3f" % (pos[0],) if pos is not None else "", 1260 | "%.3f" % (pos[1],) if pos is not None else "", 1261 | "%.3f" % (pos[2],) if pos is not None else "", 1262 | ) 1263 | f.write(obj) 1264 | 1265 | self._log_stream = self.streaming_session(cb, completion_cb) 1266 | gcmd.respond_info("Beacon Streaming enabled") 1267 | 1268 | cmd_PROBE_ACCURACY_help = "Probe Z-height accuracy at current XY position" 1269 | 1270 | def cmd_PROBE_ACCURACY(self, gcmd): 1271 | speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.0) 1272 | lift_speed = self.get_lift_speed(gcmd) 1273 | sample_count = gcmd.get_int("SAMPLES", 10, minval=1) 1274 | sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST", 0) 1275 | allow_faulty = gcmd.get_int("ALLOW_FAULTY_COORDINATE", 0) != 0 1276 | pos = self.toolhead.get_position() 1277 | gcmd.respond_info( 1278 | "PROBE_ACCURACY at X:%.3f Y:%.3f Z:%.3f" 1279 | " (samples=%d retract=%.3f" 1280 | " speed=%.1f lift_speed=%.1f)\n" 1281 | % ( 1282 | pos[0], 1283 | pos[1], 1284 | pos[2], 1285 | sample_count, 1286 | sample_retract_dist, 1287 | speed, 1288 | lift_speed, 1289 | ) 1290 | ) 1291 | 1292 | start_height = self.trigger_distance + sample_retract_dist 1293 | liftpos = [None, None, start_height] 1294 | self.toolhead.manual_move(liftpos, lift_speed) 1295 | 1296 | self.multi_probe_begin() 1297 | positions = [] 1298 | while len(positions) < sample_count: 1299 | pos = self._probe(speed, allow_faulty=allow_faulty) 1300 | positions.append(pos) 1301 | self.toolhead.manual_move(liftpos, lift_speed) 1302 | self.multi_probe_end() 1303 | 1304 | zs = [p[2] for p in positions] 1305 | max_value = max(zs) 1306 | min_value = min(zs) 1307 | range_value = max_value - min_value 1308 | avg_value = sum(zs) / len(positions) 1309 | median_ = median(zs) 1310 | 1311 | deviation_sum = 0 1312 | for i in range(len(zs)): 1313 | deviation_sum += pow(zs[2] - avg_value, 2.0) 1314 | sigma = (deviation_sum / len(zs)) ** 0.5 1315 | 1316 | gcmd.respond_info( 1317 | "probe accuracy results: maximum %.6f, minimum %.6f, range %.6f, " 1318 | "average %.6f, median %.6f, standard deviation %.6f" 1319 | % (max_value, min_value, range_value, avg_value, median_, sigma) 1320 | ) 1321 | 1322 | cmd_Z_OFFSET_APPLY_PROBE_help = "Adjust the probe's z_offset" 1323 | 1324 | def cmd_Z_OFFSET_APPLY_PROBE(self, gcmd): 1325 | gcode_move = self.printer.lookup_object("gcode_move") 1326 | offset = gcode_move.get_status()["homing_origin"].z 1327 | 1328 | if offset == 0: 1329 | self.gcode.respond_info("Nothing to do: Z Offset is 0") 1330 | return 1331 | 1332 | if not self.model: 1333 | raise self.gcode.error( 1334 | "You must calibrate your model first, use BEACON_CALIBRATE." 1335 | ) 1336 | 1337 | # We use the model code to save the new offset, but we can't actually 1338 | # apply that offset yet because the gcode_offset is still in effect. 1339 | # If the user continues to do stuff after this, the newly set model 1340 | # offset would compound with the gcode offset. To ensure this doesn't 1341 | # happen, we revert to the old model offset afterwards. 1342 | # Really, the user should just be calling `SAVE_CONFIG` now. 1343 | old_offset = self.model.offset 1344 | self.model.offset += offset 1345 | self.model.save(self, False) 1346 | gcmd.respond_info( 1347 | "Beacon model offset has been updated, new value is %.5f\n" 1348 | "You must run the SAVE_CONFIG command now to update the\n" 1349 | "printer config file and restart the printer." % (self.model.offset,) 1350 | ) 1351 | self.model.offset = old_offset 1352 | 1353 | cmd_BEACON_POKE_help = "Poke the bed" 1354 | 1355 | def cmd_BEACON_POKE(self, gcmd): 1356 | top = gcmd.get_float("TOP", 5) 1357 | bottom = gcmd.get_float("BOTTOM", -0.3) 1358 | speed = gcmd.get_float("SPEED", 3, maxval=self.autocal_max_speed) 1359 | 1360 | pos = self.toolhead.get_position() 1361 | gcmd.respond_info( 1362 | "Poke test at (%.3f,%.3f), from %.3f to %.3f, at %.3f mm/s" 1363 | % (pos[0], pos[1], top, bottom, speed) 1364 | ) 1365 | 1366 | self.last_probe_result = "failed" 1367 | self.toolhead.manual_move([None, None, top], 100.0) 1368 | self.toolhead.wait_moves() 1369 | self.toolhead.dwell(0.5) 1370 | 1371 | ts = time.strftime("%Y%m%d_%H%M%S") 1372 | fn = "/tmp/poke_%s_%.3f_%.3f-%.3f.csv" % (ts, speed, top, bottom) 1373 | with open(fn, "w") as f: 1374 | f.write("time,data,data_smooth,freq,dist,temp,pos_x,pos_y,pos_z\n") 1375 | 1376 | def cb(sample): 1377 | pos = sample.get("pos", None) 1378 | obj = "%.6f,%d,%.2f,%.5f,%.5f,%.2f,%s,%s,%s\n" % ( 1379 | sample["time"], 1380 | sample["data"], 1381 | sample["data_smooth"], 1382 | sample["freq"], 1383 | sample["dist"], 1384 | sample["temp"], 1385 | "%.3f" % (pos[0],) if pos is not None else "", 1386 | "%.3f" % (pos[1],) if pos is not None else "", 1387 | "%.5f" % (pos[2],) if pos is not None else "", 1388 | ) 1389 | f.write(obj) 1390 | 1391 | with self.streaming_session(cb): 1392 | self._sample_async() 1393 | self.toolhead.get_last_move_time() 1394 | pos = self.toolhead.get_position() 1395 | self.mcu_contact_probe.activate_gcode.run_gcode_from_command() 1396 | try: 1397 | hmove = HomingMove( 1398 | self.printer, [(self.mcu_contact_probe, "contact")] 1399 | ) 1400 | pos[2] = bottom 1401 | epos = hmove.homing_move(pos, speed, probe_pos=True)[:3] 1402 | self.toolhead.wait_moves() 1403 | spos = self.toolhead.get_position()[:3] 1404 | armpos = self._get_position_at_time( 1405 | self._clock32_to_time(self.last_contact_msg["armed_clock"]) 1406 | ) 1407 | gcmd.respond_info("Armed at: z=%.5f" % (armpos[2],)) 1408 | gcmd.respond_info( 1409 | "Triggered at: z=%.5f with latency=%d" 1410 | % (epos[2], self.last_contact_msg["latency"]) 1411 | ) 1412 | gcmd.respond_info( 1413 | "Overshoot: %.3f um" % ((epos[2] - spos[2]) * 1000.0,) 1414 | ) 1415 | self.last_probe_result = "ok" 1416 | self.last_poke_result = { 1417 | "target_position": pos, 1418 | "arming_z": armpos[2], 1419 | "trigger_z": epos[2], 1420 | "stopped_z": spos[2], 1421 | "latency": self.last_contact_msg["latency"], 1422 | "error": self.last_contact_msg["error"], 1423 | } 1424 | except self.printer.command_error: 1425 | if self.printer.is_shutdown(): 1426 | raise self.printer.command_error( 1427 | "Homing failed due to printer shutdown" 1428 | ) 1429 | raise 1430 | finally: 1431 | self.mcu_contact_probe.deactivate_gcode.run_gcode_from_command() 1432 | self.toolhead.manual_move([None, None, top], 100.0) 1433 | self.toolhead.wait_moves() 1434 | 1435 | cmd_BEACON_AUTO_CALIBRATE_help = "Automatically calibrates the Beacon probe" 1436 | 1437 | def cmd_BEACON_AUTO_CALIBRATE(self, gcmd): 1438 | speed = gcmd.get_float( 1439 | "SPEED", self.autocal_speed, above=0, maxval=self.autocal_max_speed 1440 | ) 1441 | desired_accel = gcmd.get_float("ACCEL", self.autocal_accel, minval=1) 1442 | retract_dist = gcmd.get_float("RETRACT", self.autocal_retract_dist, minval=1) 1443 | retract_speed = gcmd.get_float( 1444 | "RETRACT_SPEED", self.autocal_retract_speed, minval=1 1445 | ) 1446 | sample_count = gcmd.get_int("SAMPLES", self.autocal_sample_count, minval=1) 1447 | tolerance = gcmd.get_float( 1448 | "SAMPLES_TOLERANCE", self.autocal_tolerance, above=0.0 1449 | ) 1450 | max_retries = gcmd.get_int( 1451 | "SAMPLES_TOLERANCE_RETRIES", self.autocal_max_retries, minval=0 1452 | ) 1453 | 1454 | curtime = self.reactor.monotonic() 1455 | kin = self.kinematics 1456 | kin_status = kin.get_status(curtime) 1457 | if "x" not in kin_status["homed_axes"] or "y" not in kin_status["homed_axes"]: 1458 | raise gcmd.error("Must home X and Y axes first") 1459 | 1460 | self.last_probe_result = "failed" 1461 | force_pos = self.toolhead.get_position()[:] 1462 | home_pos = force_pos[:] 1463 | amin, amax = kin_status["axis_minimum"][2], kin_status["axis_maximum"][2] 1464 | force_pos[2] = amax 1465 | home_pos[2] = amin 1466 | 1467 | stop_samples = [] 1468 | 1469 | old_max_accel = self.toolhead.get_status(curtime)["max_accel"] 1470 | gcode = self.printer.lookup_object("gcode") 1471 | 1472 | def set_max_accel(value): 1473 | gcode.run_script_from_command("SET_VELOCITY_LIMIT ACCEL=%.3f" % (value,)) 1474 | 1475 | homing_state = BeaconHomingState() 1476 | self.printer.send_event("homing:home_rails_begin", homing_state, []) 1477 | self.mcu_contact_probe.activate_gcode.run_gcode_from_command() 1478 | try: 1479 | self.compat_toolhead_set_position_homing_z(self.toolhead, force_pos) 1480 | skip_next = True 1481 | retries = 0 1482 | while len(stop_samples) < sample_count: 1483 | if skip_next: 1484 | gcmd.respond_info("Initial approach") 1485 | else: 1486 | gcmd.respond_info( 1487 | "Collecting sample %d/%d" 1488 | % (len(stop_samples) + 1, sample_count) 1489 | ) 1490 | self.toolhead.wait_moves() 1491 | set_max_accel(desired_accel) 1492 | try: 1493 | hmove = HomingMove( 1494 | self.printer, [(self.mcu_contact_probe, "contact")] 1495 | ) 1496 | epos = hmove.homing_move(home_pos, speed, probe_pos=True) 1497 | except self.printer.command_error: 1498 | if self.printer.is_shutdown(): 1499 | raise self.printer.command_error( 1500 | "Homing failed due to printer shutdown" 1501 | ) 1502 | raise 1503 | finally: 1504 | set_max_accel(old_max_accel) 1505 | 1506 | retract_pos = self.toolhead.get_position()[:] 1507 | retract_pos[2] += retract_dist 1508 | if retract_pos[2] > amax: 1509 | retract_pos[2] = amax 1510 | self.toolhead.move(retract_pos, retract_speed) 1511 | self.toolhead.dwell(1.0) 1512 | 1513 | if not skip_next: 1514 | stop_samples.append(epos[2]) 1515 | mean = np.mean(stop_samples) 1516 | delta = max([abs(v - mean) for v in stop_samples]) 1517 | if delta > tolerance: 1518 | if retries >= max_retries: 1519 | raise gcmd.error( 1520 | "Sample spread too large(%.4f > %.4f)" 1521 | % (delta, tolerance) 1522 | ) 1523 | gcmd.respond_info( 1524 | "Sample spread too large(%.4f > %.4f), restarting" 1525 | % (delta, tolerance) 1526 | ) 1527 | retries += 1 1528 | stop_samples = [] 1529 | skip_next = True 1530 | else: 1531 | skip_next = False 1532 | 1533 | gcmd.respond_info( 1534 | "Collected %d samples, %.4f sd" 1535 | % (len(stop_samples), np.std(stop_samples)) 1536 | ) 1537 | 1538 | current_delta = force_pos[2] - self.toolhead.get_position()[2] 1539 | true_zero_delta = force_pos[2] - np.mean(stop_samples) 1540 | 1541 | force_pos[2] = float(true_zero_delta - current_delta) 1542 | self.toolhead.set_position(force_pos) 1543 | 1544 | self.toolhead.wait_moves() 1545 | self.toolhead.flush_step_generation() 1546 | self.last_probe_result = "ok" 1547 | self.printer.send_event("homing:home_rails_end", homing_state, []) 1548 | if gcmd.get_int("SKIP_MODEL_CREATION", 0) == 0: 1549 | self._calibrate(gcmd, force_pos, force_pos[2], True, True) 1550 | 1551 | except self.printer.command_error: 1552 | self.compat_kin_note_z_not_homed(kin) 1553 | raise 1554 | finally: 1555 | self.mcu_contact_probe.deactivate_gcode.run_gcode_from_command() 1556 | 1557 | cmd_BEACON_OFFSET_COMPARE_help = ( 1558 | "Measures offset between contact and proximity measurements" 1559 | ) 1560 | 1561 | def cmd_BEACON_OFFSET_COMPARE(self, gcmd): 1562 | top = gcmd.get_float("TOP", 2) 1563 | 1564 | self.last_probe_result = "failed" 1565 | self.toolhead.get_last_move_time() 1566 | self._sample_async() 1567 | start_pos = self.toolhead.get_position() 1568 | 1569 | params = { 1570 | "SAMPLES_DROP": 1, 1571 | "SAMPLES": 3, 1572 | } 1573 | params.update(gcmd.get_command_parameters()) 1574 | 1575 | # Do contact move 1576 | epos = self._run_probe_contact( 1577 | self.gcode.create_gcode_command( 1578 | "PROBE", 1579 | "PROBE", 1580 | params, 1581 | ) 1582 | ) 1583 | 1584 | # Up 1585 | self.toolhead.manual_move([None, None, top + 0.5], 100.0) 1586 | 1587 | # Over 1588 | pos = start_pos[:2] 1589 | pos[0] -= self.x_offset 1590 | pos[1] -= self.y_offset 1591 | self.toolhead.manual_move(pos, 100.0) 1592 | self.toolhead.wait_moves() 1593 | 1594 | # Down 1595 | self.toolhead.manual_move([None, None, 2.0], 100.0) 1596 | 1597 | # Query 1598 | (dist, _samples) = self._sample(self.z_settling_time, 10) 1599 | dist = 2.0 - dist 1600 | 1601 | # Back 1602 | self.toolhead.manual_move(start_pos, 100.0) 1603 | self.toolhead.wait_moves() 1604 | 1605 | delta = epos[2] - dist 1606 | gcmd.respond_info("Comparing @ %.4f,%.4f" % (start_pos[0], start_pos[1])) 1607 | gcmd.respond_info("Contact: %.5f mm" % (epos[2],)) 1608 | gcmd.respond_info("Proximity: %.5f mm" % (dist,)) 1609 | gcmd.respond_info("Delta: %.3f um" % (delta * 1000,)) 1610 | self.last_probe_result = "ok" 1611 | self.last_offset_result = { 1612 | "position": (start_pos[0], start_pos[1], epos[2]), 1613 | "delta": delta, 1614 | } 1615 | 1616 | 1617 | class BeaconModel: 1618 | @classmethod 1619 | def load(cls, name, config, beacon): 1620 | coef = config.getfloatlist("model_coef") 1621 | temp = config.getfloat("model_temp") 1622 | domain = config.getfloatlist("model_domain", count=2) 1623 | [min_z, max_z] = config.getfloatlist("model_range", count=2) 1624 | offset = config.getfloat("model_offset", 0.0) 1625 | poly = Polynomial(coef, domain) 1626 | return BeaconModel(name, beacon, poly, temp, min_z, max_z, offset) 1627 | 1628 | def __init__(self, name, beacon, poly, temp, min_z, max_z, offset=0): 1629 | self.name = name 1630 | self.beacon = beacon 1631 | self.poly = poly 1632 | self.min_z = min_z 1633 | self.max_z = max_z 1634 | self.temp = temp 1635 | self.offset = offset 1636 | 1637 | def save(self, beacon, show_message=True): 1638 | configfile = beacon.printer.lookup_object("configfile") 1639 | sensor_name = "" if beacon.id.is_unnamed() else "sensor %s " % (beacon.id.name) 1640 | section = "beacon " + sensor_name + "model " + self.name 1641 | configfile.set(section, "model_coef", ",\n ".join(map(str, self.poly.coef))) 1642 | configfile.set(section, "model_domain", ",".join(map(str, self.poly.domain))) 1643 | configfile.set(section, "model_range", "%f,%f" % (self.min_z, self.max_z)) 1644 | configfile.set(section, "model_temp", "%f" % (self.temp)) 1645 | configfile.set(section, "model_offset", "%.5f" % (self.offset,)) 1646 | if show_message: 1647 | beacon.gcode.respond_info( 1648 | "Beacon calibration for model '%s' has " 1649 | "been updated\nfor the current session. The SAVE_CONFIG " 1650 | "command will\nupdate the printer config file and restart " 1651 | "the printer." % (self.name,) 1652 | ) 1653 | 1654 | def freq_to_dist_raw(self, freq): 1655 | [begin, end] = self.poly.domain 1656 | invfreq = 1 / freq 1657 | if invfreq > end: 1658 | return float("inf") 1659 | elif invfreq < begin: 1660 | return float("-inf") 1661 | else: 1662 | return float(self.poly(invfreq) - self.offset) 1663 | 1664 | def freq_to_dist(self, freq, temp): 1665 | if self.temp is not None and self.beacon.model_temp is not None: 1666 | freq = self.beacon.model_temp.compensate(freq, temp, self.temp) 1667 | return self.freq_to_dist_raw(freq) 1668 | 1669 | def dist_to_freq_raw(self, dist, max_e=0.00000001): 1670 | if dist < self.min_z or dist > self.max_z: 1671 | msg = ( 1672 | "Attempted to map out-of-range distance %f, valid range " 1673 | "[%.3f, %.3f]" % (dist, self.min_z, self.max_z) 1674 | ) 1675 | raise self.beacon.printer.command_error(msg) 1676 | dist += self.offset 1677 | [begin, end] = self.poly.domain 1678 | for _ in range(0, 50): 1679 | f = (end + begin) / 2 1680 | v = self.poly(f) 1681 | if abs(v - dist) < max_e: 1682 | return float(1.0 / f) 1683 | elif v < dist: 1684 | begin = f 1685 | else: 1686 | end = f 1687 | raise self.beacon.printer.command_error("Beacon model convergence error") 1688 | 1689 | def dist_to_freq(self, dist, temp, max_e=0.00000001): 1690 | freq = self.dist_to_freq_raw(dist, max_e) 1691 | if self.temp is not None and self.beacon.model_temp is not None: 1692 | freq = self.beacon.model_temp.compensate(freq, self.temp, temp) 1693 | return freq 1694 | 1695 | 1696 | class BeaconMCUTempHelper: 1697 | def __init__(self, temp_room, temp_hot, ref_room, ref_hot, adc_room, adc_hot): 1698 | self.temp_room = temp_room 1699 | self.temp_hot = temp_hot 1700 | self.ref_room = ref_room 1701 | self.ref_hot = ref_hot 1702 | self.adc_room = adc_room 1703 | self.adc_hot = adc_hot 1704 | 1705 | def compensate(self, beacon, mcu_temp, supply): 1706 | temp_mcu_uncomp = self.temp_room + (self.temp_hot - self.temp_room) * ( 1707 | mcu_temp / beacon.temp_smooth_count - self.adc_room * self.ref_room 1708 | ) / (self.adc_hot * self.ref_hot - self.adc_room * self.ref_room) 1709 | ref_comp = self.ref_room + (self.ref_hot - self.ref_room) * ( 1710 | temp_mcu_uncomp - self.temp_room 1711 | ) / (self.temp_hot - self.temp_room) 1712 | temp_mcu_comp = self.temp_room + (self.temp_hot - self.temp_room) * ( 1713 | mcu_temp / beacon.temp_smooth_count * ref_comp 1714 | - self.adc_room * self.ref_room 1715 | ) / (self.adc_hot * self.ref_hot - self.adc_room * self.ref_room) 1716 | supply_voltage = ( 1717 | 4.0 * supply * ref_comp / beacon.temp_smooth_count * beacon.inv_adc_max 1718 | ) 1719 | return (temp_mcu_comp, supply_voltage) 1720 | 1721 | @classmethod 1722 | def build_with_nvm(cls, beacon): 1723 | nvm_data = beacon.beacon_nvm_read_cmd.send([8, 65534]) 1724 | if nvm_data["offset"] == 65534: 1725 | (lower, upper) = struct.unpack("> 8) & 0xF) 1727 | temp_hot = ((lower >> 12) & 0xFF) + 0.1 * ((lower >> 20) & 0xF) 1728 | adc_room = (upper >> 8) & 0xFFF 1729 | adc_hot = (upper >> 20) & 0xFFF 1730 | (ref_room_raw, ref_hot_raw) = struct.unpack(" 0: 1988 | vk = vk + self.beta / dt * rk 1989 | self.xl = xk 1990 | self.vl = vk 1991 | return xk 1992 | 1993 | def value(self): 1994 | return self.xl 1995 | 1996 | 1997 | class StreamingHelper: 1998 | def __init__(self, beacon, callback, completion_callback, latency): 1999 | self.beacon = beacon 2000 | self.cb = callback 2001 | self.completion_cb = completion_callback 2002 | self.completion = self.beacon.reactor.completion() 2003 | 2004 | self.latency_key = None 2005 | if latency is not None: 2006 | self.latency_key = self.beacon.request_stream_latency(latency) 2007 | 2008 | self.beacon._stream_callbacks[self] = self._handle 2009 | self.beacon._start_streaming() 2010 | 2011 | def __enter__(self): 2012 | return self 2013 | 2014 | def __exit__(self, exc_type, exc_val, exc_tb): 2015 | self.stop() 2016 | 2017 | def _handle(self, sample): 2018 | try: 2019 | self.cb(sample) 2020 | except StopStreaming: 2021 | self.completion.complete(()) 2022 | 2023 | def stop(self): 2024 | if self not in self.beacon._stream_callbacks: 2025 | return 2026 | del self.beacon._stream_callbacks[self] 2027 | self.beacon._stop_streaming() 2028 | if self.latency_key is not None: 2029 | self.beacon.drop_stream_latency_request(self.latency_key) 2030 | if self.completion_cb is not None: 2031 | self.completion_cb() 2032 | 2033 | def wait(self): 2034 | self.completion.wait() 2035 | self.stop() 2036 | 2037 | 2038 | class StopStreaming(Exception): 2039 | pass 2040 | 2041 | 2042 | class BeaconProbeWrapper: 2043 | def __init__(self, beacon): 2044 | self.beacon = beacon 2045 | self.results = None 2046 | 2047 | def multi_probe_begin(self): 2048 | return self.beacon.multi_probe_begin() 2049 | 2050 | def multi_probe_end(self): 2051 | return self.beacon.multi_probe_end() 2052 | 2053 | def get_offsets(self): 2054 | return self.beacon.get_offsets() 2055 | 2056 | def get_lift_speed(self, gcmd=None): 2057 | return self.beacon.get_lift_speed(gcmd) 2058 | 2059 | def run_probe(self, gcmd, *args, **kwargs): 2060 | result = self.beacon.run_probe(gcmd) 2061 | if self.results is not None: 2062 | self.results.append(result) 2063 | return result 2064 | 2065 | def get_probe_params(self, gcmd=None): 2066 | return {"lift_speed": self.beacon.get_lift_speed(gcmd)} 2067 | 2068 | def start_probe_session(self, gcmd): 2069 | self.multi_probe_begin() 2070 | self.results = [] 2071 | return self 2072 | 2073 | def end_probe_session(self): 2074 | self.results = None 2075 | self.multi_probe_end() 2076 | 2077 | def pull_probed_results(self): 2078 | results = self.results 2079 | if results is None: 2080 | return [] 2081 | else: 2082 | self.results = [] 2083 | return results 2084 | 2085 | def get_status(self, eventtime): 2086 | return {"name": "beacon"} 2087 | 2088 | 2089 | class BeaconTempWrapper: 2090 | def __init__(self, beacon): 2091 | self.beacon = beacon 2092 | 2093 | def get_temp(self, eventtime): 2094 | return self.beacon.last_temp, 0 2095 | 2096 | def get_status(self, eventtime): 2097 | return { 2098 | "temperature": round(self.beacon.last_temp, 2), 2099 | "measured_min_temp": round(self.beacon.measured_min, 2), 2100 | "measured_max_temp": round(self.beacon.measured_max, 2), 2101 | } 2102 | 2103 | 2104 | class BeaconEndstopShared: 2105 | def __init__(self, beacon): 2106 | self.beacon = beacon 2107 | 2108 | ffi_main, ffi_lib = chelper.get_ffi() 2109 | self._trdispatch = ffi_main.gc(ffi_lib.trdispatch_alloc(), ffi_lib.free) 2110 | self._trsync = MCU_trsync(self.beacon._mcu, self._trdispatch) 2111 | self._trsyncs = [self._trsync] 2112 | 2113 | beacon.printer.register_event_handler( 2114 | "klippy:mcu_identify", self._handle_mcu_identify 2115 | ) 2116 | 2117 | def _handle_mcu_identify(self): 2118 | self.toolhead = self.beacon.printer.lookup_object("toolhead") 2119 | kin = self.toolhead.get_kinematics() 2120 | for stepper in kin.get_steppers(): 2121 | if stepper.is_active_axis("z"): 2122 | self.add_stepper(stepper) 2123 | 2124 | def add_stepper(self, stepper): 2125 | trsyncs = {trsync.get_mcu(): trsync for trsync in self._trsyncs} 2126 | stepper_mcu = stepper.get_mcu() 2127 | trsync = trsyncs.get(stepper_mcu) 2128 | if trsync is None: 2129 | trsync = MCU_trsync(stepper_mcu, self._trdispatch) 2130 | self._trsyncs.append(trsync) 2131 | trsync.add_stepper(stepper) 2132 | # Check for unsupported multi-mcu shared stepper rails, duplicated 2133 | # from MCU_endstop 2134 | sname = stepper.get_name() 2135 | if sname.startswith("stepper_"): 2136 | for ot in self._trsyncs: 2137 | for s in ot.get_steppers(): 2138 | if ot is not trsync and s.get_name().startswith(sname[:9]): 2139 | raise self.beacon.printer.config_error( 2140 | "Multi-mcu homing not supported on multi-mcu shared axis" 2141 | ) 2142 | 2143 | def get_steppers(self): 2144 | return [s for trsync in self._trsyncs for s in trsync.get_steppers()] 2145 | 2146 | def trsync_start(self, print_time): 2147 | self._trigger_completion = self.beacon.reactor.completion() 2148 | expire_timeout = self.beacon.trsync_timeout 2149 | for i, trsync in enumerate(self._trsyncs): 2150 | try: 2151 | trsync.start(print_time, self._trigger_completion, expire_timeout) 2152 | except TypeError: 2153 | offset = float(i) / len(self._trsyncs) 2154 | trsync.start( 2155 | print_time, offset, self._trigger_completion, expire_timeout 2156 | ) 2157 | ffi_main, ffi_lib = chelper.get_ffi() 2158 | ffi_lib.trdispatch_start(self._trdispatch, self._trsync.REASON_HOST_REQUEST) 2159 | 2160 | def trsync_stop(self, home_end_time): 2161 | self._trsync.set_home_end_time(home_end_time) 2162 | if self.beacon._mcu.is_fileoutput(): 2163 | self._trigger_completion.complete(True) 2164 | self._trigger_completion.wait() 2165 | ffi_main, ffi_lib = chelper.get_ffi() 2166 | ffi_lib.trdispatch_stop(self._trdispatch) 2167 | res = [trsync.stop() for trsync in self._trsyncs] 2168 | if any([r == self._trsync.REASON_COMMS_TIMEOUT for r in res]): 2169 | cmderr = self.beacon.printer.command_error 2170 | raise cmderr("Communication timeout during homing") 2171 | if res[0] != self._trsync.REASON_ENDSTOP_HIT: 2172 | return 0.0 2173 | return None 2174 | 2175 | 2176 | class BeaconEndstopWrapper: 2177 | def __init__(self, beacon): 2178 | self.beacon = beacon 2179 | self._shared = beacon._endstop_shared 2180 | 2181 | printer = beacon.printer 2182 | printer.register_event_handler( 2183 | "homing:home_rails_begin", self._handle_home_rails_begin 2184 | ) 2185 | printer.register_event_handler( 2186 | "homing:home_rails_end", self._handle_home_rails_end 2187 | ) 2188 | 2189 | self.is_homing = False 2190 | 2191 | def _handle_home_rails_begin(self, homing_state, rails): 2192 | self.is_homing = False 2193 | 2194 | def _handle_home_rails_end(self, homing_state, rails): 2195 | if self.beacon.model is None: 2196 | return 2197 | 2198 | if not self.is_homing: 2199 | return 2200 | 2201 | if 2 not in homing_state.get_axes(): 2202 | return 2203 | 2204 | # After homing Z we perform a measurement and adjust the toolhead 2205 | # kinematic position. 2206 | (dist, samples) = self.beacon._sample(self.beacon.z_settling_time, 10) 2207 | if math.isinf(dist): 2208 | logging.error("Post-homing adjustment measured samples %s", samples) 2209 | raise self.beacon.printer.command_error( 2210 | "Toolhead stopped below model range" 2211 | ) 2212 | homing_state.set_homed_position([None, None, dist]) 2213 | 2214 | def get_mcu(self): 2215 | return self.beacon._mcu 2216 | 2217 | def add_stepper(self, stepper): 2218 | self._shared.add_stepper(stepper) 2219 | 2220 | def get_steppers(self): 2221 | return self._shared.get_steppers() 2222 | 2223 | def home_start( 2224 | self, print_time, sample_time, sample_count, rest_time, triggered=True 2225 | ): 2226 | if self.beacon.model is None: 2227 | raise self.beacon.printer.command_error("No Beacon model loaded") 2228 | 2229 | self.is_homing = True 2230 | self.beacon._apply_threshold() 2231 | self.beacon._sample_async() 2232 | 2233 | self._shared.trsync_start(print_time) 2234 | 2235 | etrsync = self._shared._trsync 2236 | self.beacon.beacon_home_cmd.send( 2237 | [ 2238 | etrsync.get_oid(), 2239 | etrsync.REASON_ENDSTOP_HIT, 2240 | 0, 2241 | ] 2242 | ) 2243 | return self._shared._trigger_completion 2244 | 2245 | def home_wait(self, home_end_time): 2246 | ret = self._shared.trsync_stop(home_end_time) 2247 | self.beacon.beacon_stop_home_cmd.send() 2248 | if ret is not None: 2249 | return ret 2250 | return home_end_time 2251 | 2252 | def query_endstop(self, print_time): 2253 | if self.beacon.model is None: 2254 | return 1 2255 | self.beacon._mcu.print_time_to_clock(print_time) 2256 | sample = self.beacon._sample_async() 2257 | if self.beacon.trigger_freq <= sample["freq"]: 2258 | return 1 2259 | else: 2260 | return 0 2261 | 2262 | def get_position_endstop(self): 2263 | return self.beacon.trigger_distance 2264 | 2265 | 2266 | class BeaconContactEndstopWrapper: 2267 | def __init__(self, beacon, config): 2268 | self.beacon = beacon 2269 | self._shared = beacon._endstop_shared 2270 | 2271 | gcode_macro = beacon.printer.load_object(config, "gcode_macro") 2272 | self.activate_gcode = gcode_macro.load_template( 2273 | config, "contact_activate_gcode", "" 2274 | ) 2275 | self.deactivate_gcode = gcode_macro.load_template( 2276 | config, "contact_deactivate_gcode", "" 2277 | ) 2278 | self.max_hotend_temp = config.getfloat("contact_max_hotend_temperature", 180.0) 2279 | 2280 | def get_mcu(self): 2281 | return self.beacon._mcu 2282 | 2283 | def add_stepper(self, stepper): 2284 | self._shared.add_stepper(stepper) 2285 | 2286 | def get_steppers(self): 2287 | return self._shared.get_steppers() 2288 | 2289 | def home_start( 2290 | self, print_time, sample_time, sample_count, rest_time, triggered=True 2291 | ): 2292 | extruder = self.beacon.toolhead.get_extruder() 2293 | if extruder is not None: 2294 | curtime = self.beacon.reactor.monotonic() 2295 | cur_temp = extruder.get_heater().get_status(curtime)["temperature"] 2296 | if cur_temp >= self.max_hotend_temp: 2297 | raise self.beacon.printer.command_error( 2298 | "Current hotend temperature %.1f exceeds maximum allowed temperature %.1f" 2299 | % (cur_temp, self.max_hotend_temp) 2300 | ) 2301 | 2302 | self.is_homing = True 2303 | self.beacon._sample_async() 2304 | self._shared.trsync_start(print_time) 2305 | etrsync = self._shared._trsync 2306 | if self.beacon.beacon_contact_set_latency_min_cmd is not None: 2307 | self.beacon.beacon_contact_set_latency_min_cmd.send( 2308 | [self.beacon.contact_latency_min] 2309 | ) 2310 | if self.beacon.beacon_contact_set_sensitivity_cmd is not None: 2311 | self.beacon.beacon_contact_set_sensitivity_cmd.send( 2312 | [self.beacon.contact_sensitivity] 2313 | ) 2314 | self.beacon.beacon_contact_home_cmd.send( 2315 | [ 2316 | etrsync.get_oid(), 2317 | etrsync.REASON_ENDSTOP_HIT, 2318 | 0, 2319 | 0, 2320 | ] 2321 | ) 2322 | return self._shared._trigger_completion 2323 | 2324 | def home_wait(self, home_end_time): 2325 | try: 2326 | ret = self._shared.trsync_stop(home_end_time) 2327 | if ret is not None: 2328 | return ret 2329 | if self.beacon._mcu.is_fileoutput(): 2330 | return home_end_time 2331 | self.beacon.toolhead.wait_moves() 2332 | deadline = self.beacon.reactor.monotonic() + 0.5 2333 | while True: 2334 | ret = self.beacon.beacon_contact_query_cmd.send([]) 2335 | if ret["triggered"] == 0: 2336 | now = self.beacon.reactor.monotonic() 2337 | if now >= deadline: 2338 | raise self.beacon.printer.command_error( 2339 | "Timeout getting contact time" 2340 | ) 2341 | self.beacon.reactor.pause(now + 0.001) 2342 | continue 2343 | time = self.beacon._clock32_to_time(ret["detect_clock"]) 2344 | ffi_main, ffi_lib = chelper.get_ffi() 2345 | data = ffi_main.new("struct pull_move[1]") 2346 | count = ffi_lib.trapq_extract_old(self.beacon.trapq, data, 1, 0.0, time) 2347 | if time >= home_end_time: 2348 | return 0.0 2349 | if count: 2350 | accel = data[0].accel 2351 | if accel < 0: 2352 | logging.info("Contact triggered while decelerating") 2353 | raise self.beacon.printer.command_error( 2354 | "No trigger on probe after full movement" 2355 | ) 2356 | elif accel > 0: 2357 | raise self.beacon.printer.command_error( 2358 | "Contact triggered while accelerating" 2359 | ) 2360 | return time 2361 | finally: 2362 | self.beacon.beacon_contact_stop_home_cmd.send() 2363 | 2364 | def query_endstop(self, print_time): 2365 | return 0 2366 | 2367 | def get_position_endstop(self): 2368 | return 0 2369 | 2370 | 2371 | HOMING_AUTOCAL_CALIBRATE_ALWAYS = 0 2372 | HOMING_AUTOCAL_CALIBRATE_UNHOMED = 1 2373 | HOMING_AUTOCAL_CALIBRATE_NEVER = 2 2374 | HOMING_AUTOCAL_CALIBRATE_CHOICES = { 2375 | "always": HOMING_AUTOCAL_CALIBRATE_ALWAYS, 2376 | "unhomed": HOMING_AUTOCAL_CALIBRATE_UNHOMED, 2377 | "never": HOMING_AUTOCAL_CALIBRATE_NEVER, 2378 | } 2379 | HOMING_AUTOCAL_METHOD_CONTACT = 0 2380 | HOMING_AUTOCAL_METHOD_PROXIMITY = 1 2381 | HOMING_AUTOCAL_METHOD_PROXIMITY_IF_AVAILABLE = 2 2382 | HOMING_AUTOCAL_METHOD_CHOICES = { 2383 | "contact": HOMING_AUTOCAL_METHOD_CONTACT, 2384 | "proximity": HOMING_AUTOCAL_METHOD_PROXIMITY, 2385 | "proximity_if_available": HOMING_AUTOCAL_METHOD_PROXIMITY_IF_AVAILABLE, 2386 | } 2387 | HOMING_AUTOCAL_CHOICES_METHOD = {v: k for k, v in HOMING_AUTOCAL_METHOD_CHOICES.items()} 2388 | 2389 | 2390 | class BeaconHomingHelper: 2391 | @classmethod 2392 | def create(cls, beacon, config): 2393 | home_xy_position = config.getfloatlist("home_xy_position", None, count=2) 2394 | if home_xy_position is None: 2395 | return None 2396 | return BeaconHomingHelper(beacon, config, home_xy_position) 2397 | 2398 | def __init__(self, beacon, config, home_xy_position): 2399 | self.beacon = beacon 2400 | self.home_pos = home_xy_position 2401 | 2402 | for section in ["safe_z_home", "homing_override"]: 2403 | if config.has_section(section): 2404 | raise config.error( 2405 | "home_xy_position cannot be used with [%s]" % (section,) 2406 | ) 2407 | 2408 | self.z_hop = config.getfloat("home_z_hop", 0.0) 2409 | self.z_hop_speed = config.getfloat("home_z_hop_speed", 15.0, above=0.0) 2410 | self.xy_move_speed = config.getfloat("home_xy_move_speed", 50.0, above=0.0) 2411 | self.home_y_before_x = config.getboolean("home_y_before_x", False) 2412 | self.method = config.getchoice( 2413 | "home_method", HOMING_AUTOCAL_METHOD_CHOICES, "proximity" 2414 | ) 2415 | self.method_when_homed = config.getchoice( 2416 | "home_method_when_homed", 2417 | HOMING_AUTOCAL_METHOD_CHOICES, 2418 | HOMING_AUTOCAL_CHOICES_METHOD[self.method], 2419 | ) 2420 | self.autocal_create_model = config.getchoice( 2421 | "home_autocalibrate", HOMING_AUTOCAL_CALIBRATE_CHOICES, "always" 2422 | ) 2423 | 2424 | gcode_macro = beacon.printer.load_object(config, "gcode_macro") 2425 | self.tmpl_pre_xy = gcode_macro.load_template(config, "home_gcode_pre_xy", "") 2426 | self.tmpl_post_xy = gcode_macro.load_template(config, "home_gcode_post_xy", "") 2427 | self.tmpl_pre_x = gcode_macro.load_template(config, "home_gcode_pre_x", "") 2428 | self.tmpl_post_x = gcode_macro.load_template(config, "home_gcode_post_x", "") 2429 | self.tmpl_pre_y = gcode_macro.load_template(config, "home_gcode_pre_y", "") 2430 | self.tmpl_post_y = gcode_macro.load_template(config, "home_gcode_post_y", "") 2431 | self.tmpl_pre_z = gcode_macro.load_template(config, "home_gcode_pre_z", "") 2432 | self.tmpl_post_z = gcode_macro.load_template(config, "home_gcode_post_z", "") 2433 | 2434 | # Ensure homing is loaded so we can override G28 2435 | beacon.printer.load_object(config, "homing") 2436 | self.gcode = gcode = beacon.gcode 2437 | self.prev_gcmd = gcode.register_command("G28", None) 2438 | gcode.register_command("G28", self.cmd_G28) 2439 | 2440 | def _maybe_zhop(self, toolhead): 2441 | if self.z_hop != 0: 2442 | curtime = self.beacon.reactor.monotonic() 2443 | kin = toolhead.get_kinematics() 2444 | kin_status = kin.get_status(curtime) 2445 | pos = toolhead.get_position() 2446 | 2447 | move = [None, None, self.z_hop] 2448 | if "z" not in kin_status["homed_axes"]: 2449 | pos[2] = 0 2450 | self.beacon.compat_toolhead_set_position_homing_z(toolhead, pos) 2451 | toolhead.manual_move(move, self.z_hop_speed) 2452 | toolhead.wait_moves() 2453 | self.beacon.compat_kin_note_z_not_homed(kin) 2454 | elif pos[2] < self.z_hop: 2455 | toolhead.manual_move(move, self.z_hop_speed) 2456 | toolhead.wait_moves() 2457 | 2458 | def _run_hook(self, template, params, raw_params): 2459 | ctx = template.create_template_context() 2460 | ctx["params"] = params 2461 | ctx["rawparams"] = raw_params 2462 | template.run_gcode_from_command(ctx) 2463 | 2464 | def cmd_G28(self, gcmd): 2465 | toolhead = self.beacon.printer.lookup_object("toolhead") 2466 | orig_params = gcmd.get_command_parameters() 2467 | raw_params = gcmd.get_raw_command_parameters() 2468 | 2469 | self._maybe_zhop(toolhead) 2470 | 2471 | want_x, want_y, want_z = [gcmd.get(a, None) is not None for a in "XYZ"] 2472 | # No axes given => home them all 2473 | if not (want_x or want_y or want_z): 2474 | want_x = want_y = want_z = True 2475 | 2476 | if want_x or want_y: 2477 | self._run_hook(self.tmpl_pre_xy, orig_params, raw_params) 2478 | if self.home_y_before_x: 2479 | axis_order = "yx" 2480 | else: 2481 | axis_order = "xy" 2482 | for axis in axis_order: 2483 | if axis == "x" and want_x: 2484 | self._run_hook(self.tmpl_pre_x, orig_params, raw_params) 2485 | cmd = self.gcode.create_gcode_command("G28", "G28", {"X": "0"}) 2486 | self.prev_gcmd(cmd) 2487 | self._run_hook(self.tmpl_post_x, orig_params, raw_params) 2488 | elif axis == "y" and want_y: 2489 | self._run_hook(self.tmpl_pre_y, orig_params, raw_params) 2490 | cmd = self.gcode.create_gcode_command("G28", "G28", {"Y": "0"}) 2491 | self.prev_gcmd(cmd) 2492 | self._run_hook(self.tmpl_post_y, orig_params, raw_params) 2493 | self._run_hook(self.tmpl_post_xy, orig_params, raw_params) 2494 | 2495 | if want_z: 2496 | self._run_hook(self.tmpl_pre_z, orig_params, raw_params) 2497 | curtime = self.beacon.reactor.monotonic() 2498 | kin = toolhead.get_kinematics() 2499 | kin_status = kin.get_status(curtime) 2500 | if "xy" not in kin_status["homed_axes"]: 2501 | raise gcmd.error("Must home X and Y axes before homing Z") 2502 | 2503 | method = self.method 2504 | if "z" in kin_status["homed_axes"]: 2505 | method = self.method_when_homed 2506 | 2507 | # G28 is not normally an extended gcode, so we need this hack 2508 | args = gcmd.get_commandline().split(" ") 2509 | for arg in args: 2510 | kv = arg.split("=") 2511 | if len(kv) == 2 and kv[0].strip().lower() == "method": 2512 | method = HOMING_AUTOCAL_METHOD_CHOICES.get( 2513 | kv[1].strip().lower(), None 2514 | ) 2515 | if method is None: 2516 | raise gcmd.error( 2517 | "Invalid homing method, valid choices: proximity, proximity_if_available, contact" 2518 | ) 2519 | break 2520 | 2521 | pos = [self.home_pos[0], self.home_pos[1]] 2522 | 2523 | if method == HOMING_AUTOCAL_METHOD_PROXIMITY_IF_AVAILABLE: 2524 | if self.beacon.model is not None: 2525 | method = HOMING_AUTOCAL_METHOD_PROXIMITY 2526 | else: 2527 | method = HOMING_AUTOCAL_METHOD_CONTACT 2528 | 2529 | if method == HOMING_AUTOCAL_METHOD_CONTACT: 2530 | toolhead.manual_move(pos, self.xy_move_speed) 2531 | 2532 | calibrate = True 2533 | if self.autocal_create_model == HOMING_AUTOCAL_CALIBRATE_UNHOMED: 2534 | calibrate = "z" not in kin_status["homed_axes"] 2535 | elif self.autocal_create_model == HOMING_AUTOCAL_CALIBRATE_NEVER: 2536 | calibrate = False 2537 | 2538 | override = gcmd.get("CALIBRATE", None) 2539 | if override is not None: 2540 | if override.lower() in ["=0", "=no", "=false"]: 2541 | calibrate = False 2542 | else: 2543 | calibrate = True 2544 | 2545 | cmd = "BEACON_AUTO_CALIBRATE" 2546 | params = {} 2547 | if not calibrate: 2548 | params["SKIP_MODEL_CREATION"] = "1" 2549 | cmd = self.gcode.create_gcode_command(cmd, cmd, params) 2550 | self.beacon.cmd_BEACON_AUTO_CALIBRATE(cmd) 2551 | elif method == HOMING_AUTOCAL_METHOD_PROXIMITY: 2552 | pos[0] -= self.beacon.x_offset 2553 | pos[1] -= self.beacon.y_offset 2554 | toolhead.manual_move(pos, self.xy_move_speed) 2555 | cmd = self.gcode.create_gcode_command("G28", "G28", {"Z": "0"}) 2556 | self.prev_gcmd(cmd) 2557 | else: 2558 | raise gcmd.error("Invalid homing method '%s'" % (method,)) 2559 | self._maybe_zhop(toolhead) 2560 | self._run_hook(self.tmpl_post_z, orig_params, raw_params) 2561 | 2562 | 2563 | class BeaconHomingState: 2564 | def get_axes(self): 2565 | return [2] 2566 | 2567 | def get_trigger_position(self, stepper_name): 2568 | raise Exception("get_trigger_position not supported") 2569 | 2570 | def set_stepper_adjustment(self, stepper_name, adjustment): 2571 | pass 2572 | 2573 | def set_homed_position(self, pos): 2574 | pass 2575 | 2576 | 2577 | class BeaconMeshHelper: 2578 | @classmethod 2579 | def create(cls, beacon, config): 2580 | if config.has_section("bed_mesh"): 2581 | mesh_config = config.getsection("bed_mesh") 2582 | if mesh_config.get("mesh_radius", None) is not None: 2583 | return None # Use normal bed meshing for round beds 2584 | return BeaconMeshHelper(beacon, config, mesh_config) 2585 | else: 2586 | return None 2587 | 2588 | def __init__(self, beacon, config, mesh_config): 2589 | self.beacon = beacon 2590 | self.scipy = None 2591 | self.mesh_config = mesh_config 2592 | self.bm = self.beacon.printer.load_object(mesh_config, "bed_mesh") 2593 | 2594 | self.speed = mesh_config.getfloat("speed", 50.0, above=0.0, note_valid=False) 2595 | self.def_min_x, self.def_min_y = mesh_config.getfloatlist( 2596 | "mesh_min", count=2, note_valid=False 2597 | ) 2598 | self.def_max_x, self.def_max_y = mesh_config.getfloatlist( 2599 | "mesh_max", count=2, note_valid=False 2600 | ) 2601 | 2602 | if self.def_min_x > self.def_max_x: 2603 | self.def_min_x, self.def_max_x = self.def_max_x, self.def_min_x 2604 | if self.def_min_y > self.def_max_y: 2605 | self.def_min_y, self.def_max_y = self.def_max_y, self.def_min_y 2606 | 2607 | self.def_res_x, self.def_res_y = mesh_config.getintlist( 2608 | "probe_count", count=2, note_valid=False 2609 | ) 2610 | self.rri = mesh_config.getint( 2611 | "relative_reference_index", None, note_valid=False 2612 | ) 2613 | self.zero_ref_pos = mesh_config.getfloatlist( 2614 | "zero_reference_position", None, count=2 2615 | ) 2616 | self.zero_ref_pos_cluster_size = config.getfloat( 2617 | "zero_reference_cluster_size", 1, minval=0 2618 | ) 2619 | self.dir = config.getchoice( 2620 | "mesh_main_direction", {"x": "x", "X": "x", "y": "y", "Y": "y"}, "y" 2621 | ) 2622 | self.overscan = config.getfloat("mesh_overscan", -1, minval=0) 2623 | self.cluster_size = config.getfloat("mesh_cluster_size", 1, minval=0) 2624 | self.runs = config.getint("mesh_runs", 1, minval=1) 2625 | self.adaptive_margin = mesh_config.getfloat( 2626 | "adaptive_margin", 0, note_valid=False 2627 | ) 2628 | 2629 | contact_def_min = config.getfloatlist( 2630 | "contact_mesh_min", 2631 | default=None, 2632 | count=2, 2633 | ) 2634 | contact_def_max = config.getfloatlist( 2635 | "contact_mesh_max", 2636 | default=None, 2637 | count=2, 2638 | ) 2639 | 2640 | xo = self.beacon.x_offset 2641 | yo = self.beacon.y_offset 2642 | 2643 | def_contact_min = contact_def_min 2644 | if contact_def_min is None: 2645 | def_contact_min = ( 2646 | max(self.def_min_x - xo, self.def_min_x), 2647 | max(self.def_min_y - yo, self.def_min_y), 2648 | ) 2649 | 2650 | def_contact_max = contact_def_max 2651 | if contact_def_max is None: 2652 | def_contact_max = ( 2653 | min(self.def_max_x - xo, self.def_max_x), 2654 | min(self.def_max_y - yo, self.def_max_y), 2655 | ) 2656 | 2657 | min_x = def_contact_min[0] 2658 | max_x = def_contact_max[0] 2659 | min_y = def_contact_min[1] 2660 | max_y = def_contact_max[1] 2661 | self.def_contact_min = (min(min_x, max_x), min(min_y, max_y)) 2662 | self.def_contact_max = (max(min_x, max_x), max(min_y, max_y)) 2663 | 2664 | if self.zero_ref_pos is not None and self.rri is not None: 2665 | logging.info( 2666 | "beacon: both 'zero_reference_position' and " 2667 | "'relative_reference_index' options are specified. The" 2668 | " former will be used" 2669 | ) 2670 | 2671 | self.faulty_regions = [] 2672 | for i in list(range(1, 100, 1)): 2673 | start = mesh_config.getfloatlist( 2674 | "faulty_region_%d_min" % (i,), None, count=2 2675 | ) 2676 | if start is None: 2677 | break 2678 | end = mesh_config.getfloatlist("faulty_region_%d_max" % (i,), count=2) 2679 | x_min = min(start[0], end[0]) 2680 | x_max = max(start[0], end[0]) 2681 | y_min = min(start[1], end[1]) 2682 | y_max = max(start[1], end[1]) 2683 | self.faulty_regions.append(Region(x_min, x_max, y_min, y_max)) 2684 | 2685 | self.exclude_object = None 2686 | beacon.printer.register_event_handler("klippy:connect", self._handle_connect) 2687 | 2688 | self.gcode = beacon.gcode 2689 | self.prev_gcmd = self.gcode.register_command("BED_MESH_CALIBRATE", None) 2690 | self.gcode.register_command( 2691 | "BED_MESH_CALIBRATE", 2692 | self.cmd_BED_MESH_CALIBRATE, 2693 | desc=self.cmd_BED_MESH_CALIBRATE_help, 2694 | ) 2695 | 2696 | cmd_BED_MESH_CALIBRATE_help = "Perform Mesh Bed Leveling" 2697 | 2698 | def cmd_BED_MESH_CALIBRATE(self, gcmd): 2699 | method = gcmd.get("METHOD", "beacon").lower() 2700 | probe_method = gcmd.get( 2701 | "PROBE_METHOD", self.beacon.default_probe_method 2702 | ).lower() 2703 | if probe_method != "proximity": 2704 | method = "automatic" 2705 | if method == "beacon": 2706 | self.calibrate(gcmd) 2707 | else: 2708 | # For backwards compatibility, ZRP is specified in probe coordinates. 2709 | # When in contact mode, we need to remove the offset first 2710 | if hasattr(self.bm.bmc, "zero_ref_pos"): 2711 | zrp = self.zero_ref_pos 2712 | if zrp is not None and probe_method == "contact": 2713 | zrp = (zrp[0] + self.beacon.x_offset, zrp[1] + self.beacon.y_offset) 2714 | self.bm.bmc.zero_ref_pos = zrp 2715 | # In contact mode, clamp MESH_MIN and MESH_MAX in case they aren't given, to 2716 | # ensure the requested area is safe to probe. This results in a slightly smaller 2717 | # mesh but guarantees it can be processed. 2718 | if probe_method == "contact": 2719 | params = gcmd.get_command_parameters() 2720 | extra_params = {} 2721 | if "MESH_MIN" not in params: 2722 | extra_params["MESH_MIN"] = ",".join(map(str, self.def_contact_min)) 2723 | if "MESH_MAX" not in params: 2724 | extra_params["MESH_MAX"] = ",".join(map(str, self.def_contact_max)) 2725 | if extra_params: 2726 | extra_params.update(params) 2727 | gcmd = self.gcode.create_gcode_command( 2728 | gcmd.get_command(), 2729 | gcmd.get_commandline() 2730 | + "".join([" " + k + "=" + v for k, v in extra_params.items()]), 2731 | extra_params, 2732 | ) 2733 | self.beacon._current_probe = probe_method 2734 | self.prev_gcmd(gcmd) 2735 | 2736 | def _handle_connect(self): 2737 | self.exclude_object = self.beacon.printer.lookup_object("exclude_object", None) 2738 | 2739 | if self.overscan < 0: 2740 | # Auto determine a safe overscan amount 2741 | toolhead = self.beacon.printer.lookup_object("toolhead") 2742 | curtime = self.beacon.reactor.monotonic() 2743 | status = toolhead.get_kinematics().get_status(curtime) 2744 | xo = self.beacon.x_offset 2745 | yo = self.beacon.y_offset 2746 | settings = { 2747 | "x": { 2748 | "range": [self.def_min_x - xo, self.def_max_x - xo], 2749 | "machine": [status["axis_minimum"][0], status["axis_maximum"][0]], 2750 | "count": self.def_res_y, 2751 | }, 2752 | "y": { 2753 | "range": [self.def_min_y - yo, self.def_max_y - yo], 2754 | "machine": [status["axis_minimum"][1], status["axis_maximum"][1]], 2755 | "count": self.def_res_x, 2756 | }, 2757 | }[self.dir] 2758 | 2759 | r = settings["range"] 2760 | m = settings["machine"] 2761 | space = (r[1] - r[0]) / (float(settings["count"] - 1)) 2762 | self.overscan = min( 2763 | [ 2764 | max(0, r[0] - m[0]), 2765 | max(0, m[1] - r[1]), 2766 | space + 2.0, # A half circle with 2mm lead in/out 2767 | ] 2768 | ) 2769 | 2770 | def _generate_path(self): 2771 | xo = self.beacon.x_offset 2772 | yo = self.beacon.y_offset 2773 | settings = { 2774 | "x": { 2775 | "range_aligned": [self.min_x - xo, self.max_x - xo], 2776 | "range_perpendicular": [self.min_y - yo, self.max_y - yo], 2777 | "count": self.res_y, 2778 | "swap_coord": False, 2779 | }, 2780 | "y": { 2781 | "range_aligned": [self.min_y - yo, self.max_y - yo], 2782 | "range_perpendicular": [self.min_x - xo, self.max_x - xo], 2783 | "count": self.res_x, 2784 | "swap_coord": True, 2785 | }, 2786 | }[self.dir] 2787 | 2788 | # We build the path in "normalized" coordinates and then simply 2789 | # swap x and y at the end if we need to 2790 | begin_a, end_a = settings["range_aligned"] 2791 | begin_p, end_p = settings["range_perpendicular"] 2792 | swap_coord = settings["swap_coord"] 2793 | step = (end_p - begin_p) / (float(settings["count"] - 1)) 2794 | points = [] 2795 | corner_radius = min(step / 2, self.overscan) 2796 | for i in range(0, settings["count"]): 2797 | pos_p = begin_p + step * i 2798 | even = i % 2 == 0 # If even we are going 'right', else 'left' 2799 | pa = (begin_a, pos_p) if even else (end_a, pos_p) 2800 | pb = (end_a, pos_p) if even else (begin_a, pos_p) 2801 | 2802 | line = (pa, pb) 2803 | 2804 | if len(points) > 0 and corner_radius > 0: 2805 | # We need to insert an overscan corner. Basically we insert 2806 | # a rounded rectangle to smooth out the transition and retain 2807 | # as much speed as we can. 2808 | # 2809 | # ---|---< 2810 | # / 2811 | # | 2812 | # \ 2813 | # ---|---> 2814 | # 2815 | # We just need to draw the two 90 degree arcs. They contain 2816 | # the endpoints of the lines connecting everything. 2817 | if even: 2818 | center = begin_a - self.overscan + corner_radius 2819 | points += arc_points( 2820 | center, pos_p - step + corner_radius, corner_radius, -90, -90 2821 | ) 2822 | points += arc_points( 2823 | center, pos_p - corner_radius, corner_radius, -180, -90 2824 | ) 2825 | else: 2826 | center = end_a + self.overscan - corner_radius 2827 | points += arc_points( 2828 | center, pos_p - step + corner_radius, corner_radius, -90, 90 2829 | ) 2830 | points += arc_points( 2831 | center, pos_p - corner_radius, corner_radius, 0, 90 2832 | ) 2833 | 2834 | points.append(line[0]) 2835 | points.append(line[1]) 2836 | 2837 | if swap_coord: 2838 | for i in range(len(points)): 2839 | (x, y) = points[i] 2840 | points[i] = (y, x) 2841 | 2842 | return points 2843 | 2844 | def calibrate(self, gcmd): 2845 | use_full = gcmd.get_int("USE_CONTACT_AREA", 0) == 0 2846 | self.min_x, self.min_y = coord_fallback( 2847 | gcmd, 2848 | "MESH_MIN", 2849 | float_parse, 2850 | self.def_min_x if use_full else self.def_contact_min[0], 2851 | self.def_min_y if use_full else self.def_contact_min[1], 2852 | lambda v, d: max(v, d), 2853 | ) 2854 | self.max_x, self.max_y = coord_fallback( 2855 | gcmd, 2856 | "MESH_MAX", 2857 | float_parse, 2858 | self.def_max_x if use_full else self.def_contact_max[0], 2859 | self.def_max_y if use_full else self.def_contact_max[1], 2860 | lambda v, d: min(v, d), 2861 | ) 2862 | self.res_x, self.res_y = coord_fallback( 2863 | gcmd, 2864 | "PROBE_COUNT", 2865 | int, 2866 | self.def_res_x, 2867 | self.def_res_y, 2868 | lambda v, _d: max(v, 3), 2869 | ) 2870 | self.profile_name = gcmd.get("PROFILE", "default") 2871 | 2872 | if self.min_x > self.max_x: 2873 | self.min_x, self.max_x = ( 2874 | max(self.max_x, self.def_min_x), 2875 | min(self.min_x, self.def_max_x), 2876 | ) 2877 | if self.min_y > self.max_y: 2878 | self.min_y, self.max_y = ( 2879 | max(self.max_y, self.def_min_y), 2880 | min(self.min_y, self.def_max_y), 2881 | ) 2882 | 2883 | # If the user gave RRI _on gcode_ then use it, else use zero_ref_pos 2884 | # if we have it, and finally use config RRI if we have it. 2885 | rri = gcmd.get_int("RELATIVE_REFERENCE_INDEX", None) 2886 | if rri is not None: 2887 | self.zero_ref_mode = ("rri", rri) 2888 | elif self.zero_ref_pos is not None: 2889 | self.zero_ref_mode = ("pos", self.zero_ref_pos) 2890 | self.zero_ref_val = None 2891 | self.zero_ref_bin = [] 2892 | elif self.rri is not None: 2893 | self.zero_ref_mode = ("rri", self.rri) 2894 | else: 2895 | self.zero_ref_mode = None 2896 | 2897 | # If the user requested adaptive meshing, try to shrink the values we just configured 2898 | if gcmd.get_int("ADAPTIVE", 0): 2899 | if self.exclude_object is not None: 2900 | margin = gcmd.get_float("ADAPTIVE_MARGIN", self.adaptive_margin) 2901 | self._shrink_to_excluded_objects(gcmd, margin) 2902 | else: 2903 | gcmd.respond_info( 2904 | "Requested adaptive mesh, but [exclude_object] is not enabled. Ignoring." 2905 | ) 2906 | 2907 | self.step_x = (self.max_x - self.min_x) / (self.res_x - 1) 2908 | self.step_y = (self.max_y - self.min_y) / (self.res_y - 1) 2909 | 2910 | self.toolhead = self.beacon.toolhead 2911 | path = self._generate_path() 2912 | 2913 | probe_speed = gcmd.get_float("PROBE_SPEED", self.beacon.speed, above=0.0) 2914 | self.beacon._move_to_probing_height(probe_speed) 2915 | 2916 | speed = gcmd.get_float("SPEED", self.speed, above=0.0) 2917 | runs = gcmd.get_int("RUNS", self.runs, minval=1) 2918 | 2919 | try: 2920 | self.beacon._start_streaming() 2921 | 2922 | # Move to first location 2923 | (x, y) = path[0] 2924 | self.toolhead.manual_move([x, y, None], speed) 2925 | self.toolhead.wait_moves() 2926 | 2927 | self.beacon._sample_printtime_sync(5) 2928 | clusters = self._sample_mesh(gcmd, path, speed, runs) 2929 | 2930 | if self.zero_ref_mode and self.zero_ref_mode[0] == "pos": 2931 | # If we didn't collect anything, hop over to the zero point 2932 | # and sample. Otherwise, grab the median of what we collected. 2933 | if len(self.zero_ref_bin) == 0: 2934 | self._collect_zero_ref(speed, self.zero_ref_mode[1]) 2935 | else: 2936 | self.zero_ref_val = median(self.zero_ref_bin) 2937 | 2938 | finally: 2939 | self.beacon._stop_streaming() 2940 | 2941 | matrix = self._process_clusters(clusters, gcmd) 2942 | self._apply_mesh(matrix, gcmd) 2943 | 2944 | def _shrink_to_excluded_objects(self, gcmd, margin): 2945 | bound_min_x, bound_max_x = None, None 2946 | bound_min_y, bound_max_y = None, None 2947 | objects = self.exclude_object.get_status().get("objects", {}) 2948 | if len(objects) == 0: 2949 | return 2950 | 2951 | for obj in objects: 2952 | for point in obj["polygon"]: 2953 | bound_min_x = opt_min(bound_min_x, point[0]) 2954 | bound_max_x = opt_max(bound_max_x, point[0]) 2955 | bound_min_y = opt_min(bound_min_y, point[1]) 2956 | bound_max_y = opt_max(bound_max_y, point[1]) 2957 | bound_min_x -= margin 2958 | bound_max_x += margin 2959 | bound_min_y -= margin 2960 | bound_max_y += margin 2961 | 2962 | # Calculate original step size and apply the new bounds 2963 | orig_span_x = self.max_x - self.min_x 2964 | orig_span_y = self.max_y - self.min_y 2965 | 2966 | if bound_min_x >= self.min_x: 2967 | self.min_x = bound_min_x 2968 | if bound_max_x <= self.max_x: 2969 | self.max_x = bound_max_x 2970 | if bound_min_y >= self.min_y: 2971 | self.min_y = bound_min_y 2972 | if bound_max_y <= self.max_y: 2973 | self.max_y = bound_max_y 2974 | 2975 | # Update resolution to retain approximately the same step size as before 2976 | self.res_x = int( 2977 | math.ceil(self.res_x * (self.max_x - self.min_x) / orig_span_x) 2978 | ) 2979 | self.res_y = int( 2980 | math.ceil(self.res_y * (self.max_y - self.min_y) / orig_span_y) 2981 | ) 2982 | # Guard against bicubic interpolation with 3 points on one axis 2983 | min_res = 3 2984 | if max(self.res_x, self.res_y) > 6 and min(self.res_x, self.res_y) < 4: 2985 | min_res = 4 2986 | self.res_x = max(self.res_x, min_res) 2987 | self.res_y = max(self.res_y, min_res) 2988 | 2989 | self.profile_name = None 2990 | 2991 | def _fly_path(self, path, speed, runs): 2992 | # Run through the path 2993 | for i in range(runs): 2994 | p = path if i % 2 == 0 else reversed(path) 2995 | for x, y in p: 2996 | self.toolhead.manual_move([x, y, None], speed) 2997 | self.toolhead.dwell(0.251) 2998 | self.toolhead.wait_moves() 2999 | 3000 | def _collect_zero_ref(self, speed, coord): 3001 | xo, yo = self.beacon.x_offset, self.beacon.y_offset 3002 | (x, y) = coord 3003 | self.toolhead.manual_move([x - xo, y - yo, None], speed) 3004 | (dist, _samples) = self.beacon._sample(50, 10) 3005 | self.zero_ref_val = dist 3006 | 3007 | def _is_valid_position(self, x, y): 3008 | return self.min_x <= x <= self.max_x and self.min_y <= y <= self.min_y 3009 | 3010 | def _is_faulty_coordinate(self, x, y, add_offsets=False): 3011 | if add_offsets: 3012 | xo, yo = self.beacon.x_offset, self.beacon.y_offset 3013 | x += xo 3014 | y += yo 3015 | for r in self.faulty_regions: 3016 | if r.is_point_within(x, y): 3017 | return True 3018 | return False 3019 | 3020 | def _sample_mesh(self, gcmd, path, speed, runs): 3021 | cs = gcmd.get_float("CLUSTER_SIZE", self.cluster_size, minval=0.0) 3022 | zcs = self.zero_ref_pos_cluster_size 3023 | if not (self.zero_ref_mode and self.zero_ref_mode[0] == "pos"): 3024 | zcs = 0 3025 | 3026 | min_x, min_y = self.min_x, self.min_y 3027 | xo, yo = self.beacon.x_offset, self.beacon.y_offset 3028 | 3029 | clusters = {} 3030 | total_samples = [0] 3031 | invalid_samples = [0] 3032 | 3033 | def cb(sample): 3034 | total_samples[0] += 1 3035 | d = sample["dist"] 3036 | (x, y, z) = sample["pos"] 3037 | x += xo 3038 | y += yo 3039 | 3040 | if d is None or math.isinf(d): 3041 | if self._is_valid_position(x, y): 3042 | invalid_samples[0] += 1 3043 | return 3044 | 3045 | # Calculate coordinate of the cluster we are in 3046 | xi = int(round((x - min_x) / self.step_x)) 3047 | yi = int(round((y - min_y) / self.step_y)) 3048 | if xi < 0 or self.res_x <= xi or yi < 0 or self.res_y <= yi: 3049 | return 3050 | 3051 | # If there's a cluster size limit, apply it here 3052 | if cs > 0: 3053 | xf = xi * self.step_x + min_x 3054 | yf = yi * self.step_y + min_y 3055 | dx = x - xf 3056 | dy = y - yf 3057 | dist = math.sqrt(dx * dx + dy * dy) 3058 | if dist > cs: 3059 | return 3060 | 3061 | # If we are looking for a zero reference, check if we 3062 | # are close enough and if so, add to the bin. 3063 | if zcs > 0: 3064 | dx = x - self.zero_ref_mode[1][0] 3065 | dy = y - self.zero_ref_mode[1][1] 3066 | dist = math.sqrt(dx * dx + dy * dy) 3067 | if dist <= zcs: 3068 | self.zero_ref_bin.append(d) 3069 | 3070 | k = (xi, yi) 3071 | 3072 | if k not in clusters: 3073 | clusters[k] = [] 3074 | clusters[k].append(d) 3075 | 3076 | with self.beacon.streaming_session(cb): 3077 | self._fly_path(path, speed, runs) 3078 | 3079 | gcmd.respond_info( 3080 | "Sampled %d total points over %d runs" % (total_samples[0], runs) 3081 | ) 3082 | if invalid_samples[0]: 3083 | gcmd.respond_info( 3084 | "!! Encountered %d invalid samples!" % (invalid_samples[0],) 3085 | ) 3086 | gcmd.respond_info("Samples binned in %d clusters" % (len(clusters),)) 3087 | 3088 | return clusters 3089 | 3090 | def _process_clusters(self, raw_clusters, gcmd): 3091 | parent_conn, child_conn = multiprocessing.Pipe() 3092 | dump_file = gcmd.get("FILENAME", None) 3093 | 3094 | def do(): 3095 | try: 3096 | child_conn.send( 3097 | (False, self._do_process_clusters(raw_clusters, dump_file)) 3098 | ) 3099 | except Exception: 3100 | child_conn.send((True, traceback.format_exc())) 3101 | child_conn.close() 3102 | 3103 | child = multiprocessing.Process(target=do) 3104 | child.daemon = True 3105 | child.start() 3106 | reactor = self.beacon.reactor 3107 | eventtime = reactor.monotonic() 3108 | while child.is_alive(): 3109 | eventtime = reactor.pause(eventtime + 0.1) 3110 | is_err, result = parent_conn.recv() 3111 | child.join() 3112 | parent_conn.close() 3113 | if is_err: 3114 | raise Exception("Error processing mesh: %s" % (result,)) 3115 | else: 3116 | is_inner_err, inner_result = result 3117 | if is_inner_err: 3118 | raise gcmd.error(inner_result) 3119 | else: 3120 | return inner_result 3121 | 3122 | def _do_process_clusters(self, raw_clusters, dump_file): 3123 | if dump_file: 3124 | with open(dump_file, "w") as f: 3125 | f.write("x,y,xp,xy,dist\n") 3126 | for yi in range(self.res_y): 3127 | for xi in range(self.res_x): 3128 | cluster = raw_clusters.get((xi, yi), []) 3129 | xp = xi * self.step_x + self.min_x 3130 | yp = yi * self.step_y + self.min_y 3131 | for dist in cluster: 3132 | f.write("%d,%d,%f,%f,%f\n" % (xi, yi, xp, yp, dist)) 3133 | 3134 | mask = self._generate_fault_mask() 3135 | matrix, faulty_regions = self._generate_matrix(raw_clusters, mask) 3136 | if len(faulty_regions) > 0: 3137 | (error, interpolator_or_msg) = self._load_interpolator() 3138 | if error: 3139 | return (True, interpolator_or_msg) 3140 | matrix = self._interpolate_faulty( 3141 | matrix, faulty_regions, interpolator_or_msg 3142 | ) 3143 | err = self._check_matrix(matrix) 3144 | if err is not None: 3145 | return (True, err) 3146 | return (False, self._finalize_matrix(matrix)) 3147 | 3148 | def _generate_fault_mask(self): 3149 | if len(self.faulty_regions) == 0: 3150 | return None 3151 | mask = np.full((self.res_y, self.res_x), True) 3152 | for r in self.faulty_regions: 3153 | r_xmin = max(0, int(math.ceil((r.x_min - self.min_x) / self.step_x))) 3154 | r_ymin = max(0, int(math.ceil((r.y_min - self.min_y) / self.step_y))) 3155 | r_xmax = min( 3156 | self.res_x - 1, int(math.floor((r.x_max - self.min_x) / self.step_x)) 3157 | ) 3158 | r_ymax = min( 3159 | self.res_y - 1, int(math.floor((r.y_max - self.min_y) / self.step_y)) 3160 | ) 3161 | for y in range(r_ymin, r_ymax + 1): 3162 | for x in range(r_xmin, r_xmax + 1): 3163 | mask[(y, x)] = False 3164 | return mask 3165 | 3166 | def _generate_matrix(self, raw_clusters, mask): 3167 | faulty_indexes = [] 3168 | matrix = np.empty((self.res_y, self.res_x)) 3169 | for (x, y), values in raw_clusters.items(): 3170 | if mask is None or mask[(y, x)]: 3171 | matrix[(y, x)] = self.beacon.trigger_distance - median(values) 3172 | else: 3173 | matrix[(y, x)] = np.nan 3174 | faulty_indexes.append((y, x)) 3175 | return matrix, faulty_indexes 3176 | 3177 | def _load_interpolator(self): 3178 | if not self.scipy: 3179 | try: 3180 | self.scipy = importlib.import_module("scipy") 3181 | except ImportError: 3182 | msg = ( 3183 | "Could not load `scipy`. To install it, simply re-run " 3184 | "the Beacon `install.sh` script. This module is required " 3185 | "when using faulty regions when bed meshing." 3186 | ) 3187 | return (True, msg) 3188 | if hasattr(self.scipy.interpolate, "RBFInterpolator"): 3189 | 3190 | def rbf_interp(points, values, faulty): 3191 | return self.scipy.interpolate.RBFInterpolator(points, values, 64)( 3192 | faulty 3193 | ) 3194 | 3195 | return (False, rbf_interp) 3196 | else: 3197 | 3198 | def linear_interp(points, values, faulty): 3199 | return self.scipy.interpolate.griddata( 3200 | points, values, faulty, method="linear" 3201 | ) 3202 | 3203 | def _cluster_mean(self, data): 3204 | median_count = max(0, int(math.floor(len(data) / 6))) 3205 | return float(np.mean(np.sort(data)[median_count : len(data) - median_count])) 3206 | 3207 | def _interpolate_faulty(self, matrix, faulty_indexes, interpolator): 3208 | ys, xs = np.mgrid[0 : matrix.shape[0], 0 : matrix.shape[1]] 3209 | points = np.array([ys.flatten(), xs.flatten()]).T 3210 | values = matrix.reshape(-1) 3211 | good = ~np.isnan(values) 3212 | fixed = interpolator(points[good], values[good], faulty_indexes) 3213 | matrix[tuple(np.array(faulty_indexes).T)] = fixed 3214 | return matrix 3215 | 3216 | def _check_matrix(self, matrix): 3217 | empty_clusters = [] 3218 | for yi in range(self.res_y): 3219 | for xi in range(self.res_x): 3220 | if np.isnan(matrix[(yi, xi)]): 3221 | xc = xi * self.step_x + self.min_x 3222 | yc = yi * self.step_y + self.min_y 3223 | empty_clusters.append(" (%.3f,%.3f)[%d,%d]" % (xc, yc, xi, yi)) 3224 | if empty_clusters: 3225 | err = ( 3226 | "Empty clusters found\n" 3227 | "Try increasing mesh cluster_size or slowing down.\n" 3228 | "The following clusters were empty:\n" 3229 | ) + "\n".join(empty_clusters) 3230 | return err 3231 | else: 3232 | return None 3233 | 3234 | def _finalize_matrix(self, matrix): 3235 | z_offset = None 3236 | if self.zero_ref_mode and self.zero_ref_mode[0] == "rri": 3237 | rri = self.zero_ref_mode[1] 3238 | if rri < 0 or rri >= self.res_x * self.res_y: 3239 | rri = None 3240 | if rri is not None: 3241 | rri_x = rri % self.res_x 3242 | rri_y = int(math.floor(rri / self.res_x)) 3243 | z_offset = matrix[rri_y][rri_x] 3244 | elif self.zero_ref_mode and self.zero_ref_mode[0] == "pos": 3245 | z_offset = self.beacon.trigger_distance - self.zero_ref_val 3246 | 3247 | if z_offset is not None: 3248 | matrix = matrix - z_offset 3249 | return matrix.tolist() 3250 | 3251 | def _apply_mesh(self, matrix, gcmd): 3252 | params = self.bm.bmc.mesh_config.copy() 3253 | params["min_x"] = self.min_x 3254 | params["max_x"] = self.max_x 3255 | params["min_y"] = self.min_y 3256 | params["max_y"] = self.max_y 3257 | params["x_count"] = self.res_x 3258 | params["y_count"] = self.res_y 3259 | try: 3260 | mesh = bed_mesh.ZMesh(params) 3261 | except TypeError: 3262 | mesh = bed_mesh.ZMesh(params, self.profile_name) 3263 | try: 3264 | mesh.build_mesh(matrix) 3265 | except bed_mesh.BedMeshError as e: 3266 | raise self.gcode.error(str(e)) 3267 | self.bm.set_mesh(mesh) 3268 | self.gcode.respond_info("Mesh calibration complete") 3269 | if self.profile_name is not None: 3270 | self.bm.save_profile(self.profile_name) 3271 | 3272 | 3273 | class Region: 3274 | def __init__(self, x_min, x_max, y_min, y_max): 3275 | self.x_min = x_min 3276 | self.x_max = x_max 3277 | self.y_min = y_min 3278 | self.y_max = y_max 3279 | 3280 | def is_point_within(self, x, y): 3281 | return (x > self.x_min and x < self.x_max) and ( 3282 | y > self.y_min and y < self.y_max 3283 | ) 3284 | 3285 | 3286 | def arc_points(cx, cy, r, start_angle, span): 3287 | # Angle delta is determined by a max deviation(md) from 0.1mm: 3288 | # r * versin(d_a) < md 3289 | # versin(d_a) < md/r 3290 | # d_a < arcversin(md/r) 3291 | # d_a < arccos(1-md/r) 3292 | # We then determine how many of these we can fit in exactly 3293 | # 90 degrees(rounding up) and then determining the exact 3294 | # delta angle. 3295 | start_angle = start_angle / 180.0 * math.pi 3296 | span = span / 180.0 * math.pi 3297 | d_a = math.acos(1 - 0.1 / r) 3298 | cnt = int(math.ceil(abs(span) / d_a)) 3299 | d_a = span / float(cnt) 3300 | 3301 | points = [] 3302 | for i in range(cnt + 1): 3303 | ang = start_angle + d_a * float(i) 3304 | x = cx + math.cos(ang) * r 3305 | y = cy + math.sin(ang) * r 3306 | points.append((x, y)) 3307 | 3308 | return points 3309 | 3310 | 3311 | def coord_fallback(gcmd, name, parse, def_x, def_y, map=lambda v, d: v): 3312 | param = gcmd.get(name, None) 3313 | if param is not None: 3314 | try: 3315 | x, y = [parse(p.strip()) for p in param.split(",", 1)] 3316 | return map(x, def_x), map(y, def_y) 3317 | except Exception: 3318 | raise gcmd.error("Unable to parse parameter '%s'" % (name,)) 3319 | else: 3320 | return def_x, def_y 3321 | 3322 | 3323 | def float_parse(s): 3324 | v = float(s) 3325 | if math.isinf(v) or np.isnan(v): 3326 | raise ValueError("could not convert string to float: '%s'" % (s,)) 3327 | return v 3328 | 3329 | 3330 | def median(samples): 3331 | return float(np.median(samples)) 3332 | 3333 | 3334 | def opt_min(a, b): 3335 | if a is None: 3336 | return b 3337 | return min(a, b) 3338 | 3339 | 3340 | def opt_max(a, b): 3341 | if a is None: 3342 | return b 3343 | return max(a, b) 3344 | 3345 | 3346 | GRAVITY = 9.80655 3347 | ACCEL_BYTES_PER_SAMPLE = 6 3348 | 3349 | Accel_Measurement = collections.namedtuple( 3350 | "Accel_Measurement", ("time", "accel_x", "accel_y", "accel_z") 3351 | ) 3352 | 3353 | 3354 | class BeaconAccelDummyConfig(object): 3355 | def __init__(self, beacon, accel_config): 3356 | self.beacon = beacon 3357 | self.accel_config = accel_config 3358 | 3359 | def get_name(self): 3360 | if self.beacon.id.is_unnamed(): 3361 | return "beacon" 3362 | else: 3363 | return "beacon_" + self.beacon.id.name 3364 | 3365 | def has_section(self, name): 3366 | if not self.beacon.id.is_unnamed(): 3367 | return True 3368 | return name == "adxl345" and self.accel_config.adxl345_exists 3369 | 3370 | def get_printer(self): 3371 | return self.beacon.printer 3372 | 3373 | 3374 | class BeaconAccelConfig(object): 3375 | def __init__(self, config): 3376 | self.default_scale = config.get("accel_scale", "") 3377 | axes = { 3378 | "x": (0, 1), 3379 | "-x": (0, -1), 3380 | "y": (1, 1), 3381 | "-y": (1, -1), 3382 | "z": (2, 1), 3383 | "-z": (2, -1), 3384 | } 3385 | axes_map = config.getlist("accel_axes_map", ("x", "y", "z"), count=3) 3386 | self.axes_map = [] 3387 | for a in axes_map: 3388 | a = a.strip() 3389 | if a not in axes: 3390 | raise config.error("Invalid accel_axes_map, unknown axes '%s'" % (a,)) 3391 | self.axes_map.append(axes[a]) 3392 | 3393 | self.adxl345_exists = config.has_section("adxl345") 3394 | 3395 | 3396 | class BeaconAccelHelper(object): 3397 | def __init__(self, beacon, config, constants): 3398 | self.beacon = beacon 3399 | self.config = config 3400 | 3401 | self._api_dump = APIDumpHelper( 3402 | beacon.printer, 3403 | lambda: self._start_streaming() or True, 3404 | lambda _: self._stop_streaming(), 3405 | self._api_update, 3406 | ) 3407 | beacon.id.register_endpoint("beacon/dump_accel", self._handle_req_dump) 3408 | adxl345.AccelCommandHelper(BeaconAccelDummyConfig(beacon, config), self) 3409 | 3410 | self._stream_en = 0 3411 | self._raw_samples = [] 3412 | self._last_raw_sample = (0, 0, 0) 3413 | self._sample_lock = threading.Lock() 3414 | 3415 | beacon._mcu.register_response(self._handle_accel_data, "beacon_accel_data") 3416 | beacon._mcu.register_response(self._handle_accel_state, "beacon_accel_state") 3417 | 3418 | self.reinit(constants) 3419 | 3420 | def reinit(self, constants): 3421 | bits = constants.get("BEACON_ACCEL_BITS") 3422 | self._clip_values = (2 ** (bits - 1) - 1, -(2 ** (bits - 1))) 3423 | 3424 | self.accel_stream_cmd = self.beacon._mcu.lookup_command( 3425 | "beacon_accel_stream en=%c scale=%c", cq=self.beacon.cmd_queue 3426 | ) 3427 | # Ensure streaming mode is stopped 3428 | self.accel_stream_cmd.send([0, 0]) 3429 | 3430 | self._scales = self._fetch_scales(constants) 3431 | self._scale = self._select_scale() 3432 | logging.info("Selected Beacon accelerometer scale %s", self._scale["name"]) 3433 | 3434 | def _fetch_scales(self, constants): 3435 | enum = self.beacon._mcu.get_enumerations().get("beacon_accel_scales", None) 3436 | if enum is None: 3437 | return {} 3438 | 3439 | scales = {} 3440 | self.default_scale_name = self.config.default_scale 3441 | first_scale_name = None 3442 | for name, id in enum.items(): 3443 | try: 3444 | scale_val_name = "BEACON_ACCEL_SCALE_%s" % (name.upper(),) 3445 | scale_val_str = constants.get(scale_val_name) 3446 | scale_val = float(scale_val_str) 3447 | except Exception: 3448 | logging.error( 3449 | "Beacon accelerometer scale %s could not be processed", name 3450 | ) 3451 | scale_val = 1 # Values will be weird, but scale will work 3452 | 3453 | if id == 0: 3454 | first_scale_name = name 3455 | scales[name] = {"name": name, "id": id, "scale": scale_val} 3456 | 3457 | if not self.default_scale_name: 3458 | if first_scale_name is None: 3459 | logging.error("Could not determine default Beacon accelerometer scale") 3460 | else: 3461 | self.default_scale_name = first_scale_name 3462 | elif self.default_scale_name not in scales: 3463 | logging.error( 3464 | "Default Beacon accelerometer scale '%s' not found, using '%s'", 3465 | self.default_scale_name, 3466 | first_scale_name, 3467 | ) 3468 | self.default_scale_name = first_scale_name 3469 | 3470 | return scales 3471 | 3472 | def _select_scale(self): 3473 | scale = self._scales.get(self.default_scale_name, None) 3474 | if scale is None: 3475 | return {"name": "unknown", "id": 0, "scale": 1} 3476 | return scale 3477 | 3478 | def _handle_accel_data(self, params): 3479 | with self._sample_lock: 3480 | if self._stream_en: 3481 | self._raw_samples.append(params) 3482 | else: 3483 | self.accel_stream_cmd.send([0, 0]) 3484 | 3485 | def _handle_accel_state(self, params): 3486 | pass 3487 | 3488 | def _handle_req_dump(self, web_request): 3489 | cconn = self._api_dump.add_web_client( 3490 | web_request, 3491 | lambda buffer: list( 3492 | itertools.chain(*map(lambda data: data["data"], buffer)) 3493 | ), 3494 | ) 3495 | cconn.send({"header": ["time", "x", "y", "z"]}) 3496 | 3497 | # Internal helpers 3498 | 3499 | def _start_streaming(self): 3500 | if self._stream_en == 0: 3501 | self._raw_samples = [] 3502 | self.accel_stream_cmd.send([1, self._scale["id"]]) 3503 | self._stream_en += 1 3504 | 3505 | def _stop_streaming(self): 3506 | self._stream_en -= 1 3507 | if self._stream_en == 0: 3508 | self._raw_samples = [] 3509 | self.accel_stream_cmd.send([0, 0]) 3510 | 3511 | def _process_samples(self, raw_samples, last_sample): 3512 | raw = last_sample 3513 | (xp, xs), (yp, ys), (zp, zs) = self.config.axes_map 3514 | scale = self._scale["scale"] * GRAVITY 3515 | xs, ys, zs = xs * scale, ys * scale, zs * scale 3516 | 3517 | errors = 0 3518 | samples = [] 3519 | 3520 | def process_value(low, high, last_value): 3521 | raw = high << 8 | low 3522 | if raw == 0x7FFF: 3523 | # Clipped value 3524 | return self._clip_values[0 if last_value >= 0 else 1] 3525 | return raw - ((high & 0x80) << 9) 3526 | 3527 | for sample in raw_samples: 3528 | tstart = self.beacon._clock32_to_time(sample["start_clock"]) 3529 | tend = self.beacon._clock32_to_time( 3530 | sample["start_clock"] + sample["delta_clock"] 3531 | ) 3532 | data = bytearray(sample["data"]) 3533 | count = int(len(data) / ACCEL_BYTES_PER_SAMPLE) 3534 | dt = (tend - tstart) / (count - 1) 3535 | for idx in range(0, count): 3536 | base = idx * ACCEL_BYTES_PER_SAMPLE 3537 | d = data[base : base + ACCEL_BYTES_PER_SAMPLE] 3538 | dxl, dxh, dyl, dyh, dzl, dzh = d 3539 | raw = ( 3540 | process_value(dxl, dxh, raw[0]), 3541 | process_value(dyl, dyh, raw[1]), 3542 | process_value(dzl, dzh, raw[2]), 3543 | ) 3544 | if raw[0] is None or raw[1] is None or raw[2] is None: 3545 | errors += 1 3546 | samples.append(None) 3547 | else: 3548 | samples.append( 3549 | ( 3550 | tstart + dt * idx, 3551 | raw[xp] * xs, 3552 | raw[yp] * ys, 3553 | raw[zp] * zs, 3554 | ) 3555 | ) 3556 | return (samples, errors, raw) 3557 | 3558 | # APIDumpHelper callbacks 3559 | 3560 | def _api_update(self, dump_helper, eventtime): 3561 | with self._sample_lock: 3562 | raw_samples = self._raw_samples 3563 | self._raw_samples = [] 3564 | (samples, errors, last_raw_sample) = self._process_samples( 3565 | raw_samples, self._last_raw_sample 3566 | ) 3567 | if len(samples) == 0: 3568 | return 3569 | self._last_raw_sample = last_raw_sample 3570 | dump_helper.buffer.append( 3571 | { 3572 | "data": samples, 3573 | "errors": errors, 3574 | "overflows": 0, 3575 | } 3576 | ) 3577 | 3578 | # Accelerometer public interface 3579 | 3580 | def start_internal_client(self): 3581 | cli = AccelInternalClient(self.beacon.printer) 3582 | self._api_dump.add_client(cli._handle_data) 3583 | return cli 3584 | 3585 | def read_reg(self, reg): 3586 | raise self.beacon.printer.command_error("Not supported") 3587 | 3588 | def set_reg(self, reg, val, minclock=0): 3589 | raise self.beacon.printer.command_error("Not supported") 3590 | 3591 | def is_measuring(self): 3592 | return self._stream_en > 0 3593 | 3594 | 3595 | class AccelInternalClient: 3596 | def __init__(self, printer): 3597 | self.printer = printer 3598 | self.toolhead = printer.lookup_object("toolhead") 3599 | self.is_finished = False 3600 | self.request_start_time = self.request_end_time = ( 3601 | self.toolhead.get_last_move_time() 3602 | ) 3603 | self.msgs = [] 3604 | self.samples = [] 3605 | 3606 | def _handle_data(self, msgs): 3607 | if self.is_finished: 3608 | return False 3609 | if len(self.msgs) >= 10000: # Limit capture length 3610 | return False 3611 | self.msgs.extend(msgs) 3612 | return True 3613 | 3614 | # AccelQueryHelper interface 3615 | 3616 | def finish_measurements(self): 3617 | self.request_end_time = self.toolhead.get_last_move_time() 3618 | self.toolhead.wait_moves() 3619 | self.is_finished = True 3620 | 3621 | def has_valid_samples(self): 3622 | for msg in self.msgs: 3623 | data = msg["data"] 3624 | first_sample_time = data[0][0] 3625 | last_sample_time = data[-1][0] 3626 | if ( 3627 | first_sample_time > self.request_end_time 3628 | or last_sample_time < self.request_start_time 3629 | ): 3630 | continue 3631 | return True 3632 | return False 3633 | 3634 | def get_samples(self): 3635 | if not self.msgs: 3636 | return self.samples 3637 | 3638 | total = sum([len(m["data"]) for m in self.msgs]) 3639 | count = 0 3640 | self.samples = samples = [None] * total 3641 | for msg in self.msgs: 3642 | for samp_time, x, y, z in msg["data"]: 3643 | if samp_time < self.request_start_time: 3644 | continue 3645 | if samp_time > self.request_end_time: 3646 | break 3647 | samples[count] = Accel_Measurement(samp_time, x, y, z) 3648 | count += 1 3649 | del samples[count:] 3650 | return self.samples 3651 | 3652 | def write_to_file(self, filename): 3653 | def do_write(): 3654 | try: 3655 | os.nice(20) 3656 | except Exception: 3657 | pass 3658 | with open(filename, "w") as f: 3659 | f.write("#time,accel_x,accel_y,accel_z\n") 3660 | samples = self.samples or self.get_samples() 3661 | for t, accel_x, accel_y, accel_z in samples: 3662 | f.write("%.6f,%.6f,%.6f,%.6f\n" % (t, accel_x, accel_y, accel_z)) 3663 | 3664 | write_proc = multiprocessing.Process(target=do_write) 3665 | write_proc.daemon = True 3666 | write_proc.start() 3667 | 3668 | 3669 | class APIDumpHelper: 3670 | def __init__(self, printer, start, stop, update): 3671 | self.printer = printer 3672 | self.start = start 3673 | self.stop = stop 3674 | self.update = update 3675 | self.interval = 0.05 3676 | self.clients = [] 3677 | self.stream = None 3678 | self.timer = None 3679 | self.buffer = [] 3680 | 3681 | def _start_stop(self): 3682 | if not self.stream and self.clients: 3683 | self.stream = self.start() 3684 | reactor = self.printer.get_reactor() 3685 | self.timer = reactor.register_timer( 3686 | self._process, reactor.monotonic() + self.interval 3687 | ) 3688 | elif self.stream is not None and not self.clients: 3689 | self.stop(self.stream) 3690 | self.stream = None 3691 | self.printer.get_reactor().unregister_timer(self.timer) 3692 | self.timer = None 3693 | 3694 | def _process(self, eventtime): 3695 | if self.update is not None: 3696 | self.update(self, eventtime) 3697 | if self.buffer: 3698 | for cb in list(self.clients): 3699 | if not cb(self.buffer): 3700 | self.clients.remove(cb) 3701 | self._start_stop() 3702 | self.buffer = [] 3703 | return eventtime + self.interval 3704 | 3705 | def add_client(self, client): 3706 | self.clients.append(client) 3707 | self._start_stop() 3708 | 3709 | def add_web_client(self, web_request, formatter=lambda v: v): 3710 | cconn = web_request.get_client_connection() 3711 | template = web_request.get_dict("response_template", {}) 3712 | 3713 | def cb(items): 3714 | if cconn.is_closed(): 3715 | return False 3716 | tmp = dict(template) 3717 | tmp["params"] = formatter(items) 3718 | cconn.send(tmp) 3719 | return True 3720 | 3721 | self.add_client(cb) 3722 | return cconn 3723 | 3724 | 3725 | class BeaconTracker: 3726 | def __init__(self, config, printer): 3727 | self.config = config 3728 | self.printer = printer 3729 | self.sensors = {} 3730 | self.gcodes = {} 3731 | self.endpoints = {} 3732 | self.gcode = printer.lookup_object("gcode") 3733 | self.webhooks = printer.lookup_object("webhooks") 3734 | 3735 | def get_status(self, eventtime): 3736 | return {"sensors": list(self.sensors.keys())} 3737 | 3738 | def home_dir(self): 3739 | return os.path.dirname(os.path.realpath(__file__)) 3740 | 3741 | def add_sensor(self, name): 3742 | if name is None: 3743 | cfg = self.config.getsection("beacon") 3744 | else: 3745 | if not name.islower(): 3746 | raise self.config.error( 3747 | "Beacon sensor name must be all lower case, sensor name '%s' is not valid" 3748 | % (name,) 3749 | ) 3750 | cfg = self.config.getsection("beacon sensor " + name) 3751 | self.sensors[name] = sensor = BeaconProbe(cfg, BeaconId(name, self)) 3752 | if name is None: 3753 | self.printer.add_object("probe", BeaconProbeWrapper(sensor)) 3754 | coil_name = "beacon_coil" if name is None else "beacon_%s_coil" % (name,) 3755 | temp = BeaconTempWrapper(sensor) 3756 | self.printer.add_object("temperature_sensor " + coil_name, temp) 3757 | pheaters = self.printer.load_object(self.config, "heaters") 3758 | pheaters.available_sensors.append("temperature_sensor " + coil_name) 3759 | return sensor 3760 | 3761 | def get_or_add_sensor(self, name): 3762 | if name in self.sensors: 3763 | return self.sensors[name] 3764 | else: 3765 | return self.add_sensor(name) 3766 | 3767 | def register_gcode_command(self, sensor, cmd, func, desc): 3768 | if cmd not in self.gcodes: 3769 | handlers = self.gcodes[cmd] = {} 3770 | self.gcode.register_command( 3771 | cmd, lambda gcmd: self.dispatch_gcode(handlers, gcmd), desc=desc 3772 | ) 3773 | self.gcodes[cmd][sensor] = func 3774 | 3775 | def dispatch_gcode(self, handlers, gcmd): 3776 | sensor = gcmd.get("SENSOR", "") 3777 | if sensor == "": 3778 | sensor = None 3779 | handler = handlers.get(sensor, None) 3780 | if not handler: 3781 | if sensor is None: 3782 | raise gcmd.error( 3783 | "No default Beacon registered, provide SENSOR= option to select specific sensor." 3784 | ) 3785 | else: 3786 | raise gcmd.error( 3787 | "Requested sensor '%s' not found, specify a valid sensor." 3788 | % (sensor,) 3789 | ) 3790 | handler(gcmd) 3791 | 3792 | def register_endpoint(self, sensor, path, callback): 3793 | if path not in self.endpoints: 3794 | self.webhooks.register_endpoint(path, self.dispatch_webhook) 3795 | self.endpoints[path] = {} 3796 | self.endpoints[path][sensor] = callback 3797 | 3798 | def dispatch_webhook(self, req): 3799 | handlers = self.endpoints[req.method] 3800 | sensor = req.get("sensor", "") 3801 | if sensor == "": 3802 | sensor = None 3803 | handler = handlers.get(sensor, None) 3804 | if not handler: 3805 | if sensor is None: 3806 | raise req.error( 3807 | "No default Beacon registered, provide 'sensor' option to specify sensor." 3808 | ) 3809 | else: 3810 | raise req.error( 3811 | "Requested sensor '%s' not found, specify a valid or no sensor to use default" 3812 | % (sensor,) 3813 | ) 3814 | handler(req) 3815 | 3816 | 3817 | class BeaconId: 3818 | def __init__(self, name, tracker): 3819 | self.name = name 3820 | self.tracker = tracker 3821 | 3822 | def is_unnamed(self): 3823 | return self.name is None 3824 | 3825 | def register_command(self, cmd, func, desc): 3826 | self.tracker.register_gcode_command(self.name, cmd, func, desc) 3827 | 3828 | def register_endpoint(self, path, callback): 3829 | self.tracker.register_endpoint(self.name, path, callback) 3830 | 3831 | 3832 | def get_beacons(config): 3833 | printer = config.get_printer() 3834 | beacons = printer.lookup_object("beacons", None) 3835 | if beacons is None: 3836 | beacons = BeaconTracker(config, printer) 3837 | printer.add_object("beacons", beacons) 3838 | return beacons 3839 | 3840 | 3841 | def load_config(config): 3842 | return get_beacons(config).get_or_add_sensor(None) 3843 | 3844 | 3845 | def load_config_prefix(config): 3846 | beacons = get_beacons(config) 3847 | sensor = None 3848 | secname = config.get_name() 3849 | parts = secname[7:].split() 3850 | 3851 | if len(parts) != 0 and parts[0] == "sensor": 3852 | if len(parts) < 2: 3853 | raise config.error("Missing Beacon sensor name") 3854 | sensor = parts[1] 3855 | parts = parts[2:] 3856 | 3857 | beacon = beacons.get_or_add_sensor(sensor) 3858 | 3859 | if len(parts) == 0: 3860 | return beacon 3861 | 3862 | if parts[0] == "model": 3863 | if len(parts) != 2: 3864 | raise config.error("Missing Beacon model name in section '%s'" % (secname,)) 3865 | name = parts[1] 3866 | model = BeaconModel.load(name, config, beacon) 3867 | beacon._register_model(name, model) 3868 | return model 3869 | else: 3870 | raise config.error("Unknown beacon config directive '%s'" % (secname,)) 3871 | --------------------------------------------------------------------------------