• Specification
    • 1. Introduction
    • 2. Basic concepts
    • 3. Data structure description language
    • 4.1 CAN bus transport layer
    • 4.2 UDP bus transport layer
    • 4.3 MAVLink bus transport layer
    • 4.4 CANFD bus transport layer
    • 5. Application level conventions
    • 6. Application level functions
    • 7. List of standard data types
    • 8. Hardware design recommendations
    • dsdl
      • ardupilot
      • uavcan
  • Implementations
    • Libuavcan
      • Platforms
      • Tutorials
        • 1. Library build configuration
        • 2. Node initialization and startup
        • 3. Publishers and subscribers
        • 4. Services
        • 5. Timers
        • 6. Time synchronization
        • 7. Remote node reconfiguration
        • 8. Custom data types
        • 9. Node discovery
        • 10. Dynamic node ID allocation
        • 11. Firmware update
        • 12. Multithreading
        • 13. CAN acceptance filters
      • FAQ
    • Pydronecan
      • Examples
        • Automated ESC enumeration
        • Dump All Messages
        • ESC throttle control
      • Tutorials
        • 1. Setup
        • 2. Basic usage
        • 3. Advanced usage
        • 4. Parsing DSDL definitions
    • AP Periph
    • Libcanard
    • dronecan dsdlc
  • GUI Tool
    • Overview
    • Examples
    • User guide
  • Examples
    • Simple sensor node
  • Discussions
Implementations /  Pydronecan /  Examples /  ESC throttle control

ESC throttle control

This example script demonstrates how to control ESC throttle setpoint using the PyDroneCAN library. Before running the script, make sure that no other node on the bus issues contradictory ESC commands concurrently.

Writing the script

One code sample is worth 1024 words:

#!/usr/bin/env python3

import dronecan, time, math

# get command line arguments
from argparse import ArgumentParser
parser = ArgumentParser(description='ESC throttle control example')
parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate")
parser.add_argument("--node-id", default=100, type=int, help="CAN node ID")
parser.add_argument("--dna-server", action='store_true', default=False, help="run DNA server")
parser.add_argument("--send-safety", action='store_true', default=False, help="send safety off messages")
parser.add_argument("port", default=None, type=str, help="serial port")
args = parser.parse_args()
    

# Publishing setpoint values from this function; it is invoked periodically from the node thread.
def publish_throttle_setpoint():
    if args.send_safety:
        # optionally send safety off messages. These are needed for some ESCs
        message = dronecan.ardupilot.indication.SafetyState()
        message.status = message.STATUS_SAFETY_OFF
        node.broadcast(message)
        print(dronecan.to_yaml(message))

    # Generating a sine wave
    setpoint = int(512 * (math.sin(time.time()) + 2))
    # Commanding ESC with indices 0, 1, 2, 3 only
    commands = [setpoint, setpoint, setpoint, setpoint]
    message = dronecan.uavcan.equipment.esc.RawCommand(cmd=commands)
    node.broadcast(message)
    # display the message on the console in human readable format
    print(dronecan.to_yaml(message))

# Initializing a DroneCAN node instance.
node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate)

# Initializing a node monitor, so we can see what nodes are online
node_monitor = dronecan.app.node_monitor.NodeMonitor(node)

if args.dna_server:
    # optionally start a DNA server
    dynamic_node_id_allocator = dronecan.app.dynamic_node_id.CentralizedServer(node, node_monitor)

# Waiting for at least one other node to appear online
while len(node_monitor.get_all_node_id()) <= 1:
    print('Waiting for other nodes to become online...')
    node.spin(timeout=1)

print("There are %u nodes online" % len(node_monitor.get_all_node_id()))

# setup to publish ESC RawCommand at 20Hz
node.periodic(0.05, publish_throttle_setpoint)

# callback for printing ESC status message to stdout in human-readable YAML format.
node.add_handler(dronecan.uavcan.equipment.esc.Status, lambda msg: print(dronecan.to_yaml(msg)))

# Running the node until the application is terminated or until first error.
try:
    node.spin()
except KeyboardInterrupt:
    pass

Running the script

Save the above code somewhere and run it. The connected ESC will be changing their RPM in a sine pattern, slowly accelerating and decelerating. The script will periodically print output similar to this:

### Message from 124 to All  ts_mono=19376.645693  ts_real=1470440665.872391
error_count: 0
voltage: 13.2812
current: 1.3379
temperature: 313.15
rpm: 1514
power_rating_pct: 13
esc_index: 3

Using the DroneCAN GUI Tool

It is also possible to run the above script (with minor modifications) directly from the interactive console of the DroneCAN GUI Tool, because the DroneCAN GUI Tool is built on top of PyDroneCAN. In that case you won’t need to create a new node yourself in the script - just use the application’s own node, it is accessible from the interactive console. For details, please read the documentation of the DroneCAN GUI Tool.


Pydronecan

  • Examples
    • Automated ESC enumeration
    • Dump All Messages
    • ESC throttle control
  • Tutorials
    • 1. Setup
    • 2. Basic usage
    • 3. Advanced usage
    • 4. Parsing DSDL definitions

Contents

  • ESC throttle control
    • Writing the script
    • Running the script
    • Using the DroneCAN GUI Tool

License

This work is licensed under a Creative Commons Attribution 4.0 International License.
Much of the content of this site is based upon prior work by Pavel Kirienko and the UAVCAN Development Team.
  • https://dronecan.org/discord
  • https://github.com/DroneCAN
  • Report a problem with this website

Generated Sun, 02 Jun 2024 21:47:21 +0000 © DroneCAN development team

  • 1. Introduction
  • 2. Basic concepts
  • 3. Data structure description language
  • 4.1 CAN bus transport layer
  • 4.2 UDP bus transport layer
  • 4.3 MAVLink bus transport layer
  • 4.4 CANFD bus transport layer
  • 5. Application level conventions
  • 6. Application level functions
  • 7. List of standard data types
  • 8. Hardware design recommendations
  • dsdl
    • ardupilot
    • uavcan
  • Libuavcan
    • Platforms
    • Tutorials
      • 1. Library build configuration
      • 2. Node initialization and startup
      • 3. Publishers and subscribers
      • 4. Services
      • 5. Timers
      • 6. Time synchronization
      • 7. Remote node reconfiguration
      • 8. Custom data types
      • 9. Node discovery
      • 10. Dynamic node ID allocation
      • 11. Firmware update
      • 12. Multithreading
      • 13. CAN acceptance filters
    • FAQ
  • Pydronecan
    • Examples
      • Automated ESC enumeration
      • Dump All Messages
      • ESC throttle control
    • Tutorials
      • 1. Setup
      • 2. Basic usage
      • 3. Advanced usage
      • 4. Parsing DSDL definitions
  • AP Periph
  • Libcanard
  • dronecan dsdlc
  • Overview
  • Examples
  • User guide
  • Simple sensor node
Specification1. Introduction2. Basic concepts3. Data structure description language4.1 CAN bus transport layer4.2 UDP bus transport layer4.3 MAVLink bus transport layer4.4 CANFD bus transport layer5. Application level conventions6. Application level functions7. List of standard data types8. Hardware design recommendationsdsdlardupilotuavcanImplementationsLibuavcanPlatformsTutorials1. Library build configuration2. Node initialization and startup3. Publishers and subscribers4. Services5. Timers6. Time synchronization7. Remote node reconfiguration8. Custom data types9. Node discovery10. Dynamic node ID allocation11. Firmware update12. Multithreading13. CAN acceptance filtersFAQPydronecanExamplesAutomated ESC enumerationDump All MessagesESC throttle controlTutorials1. Setup2. Basic usage3. Advanced usage4. Parsing DSDL definitionsAP PeriphLibcanarddronecan dsdlcGUI ToolOverviewExamplesUser guideExamplesSimple sensor nodeDiscussions