From acee89a7bfad4ec065bd90b76c84e8b29e7a2dc5 Mon Sep 17 00:00:00 2001 From: Ezward Date: Mon, 22 Aug 2022 17:52:36 -0700 Subject: [PATCH] Port changes from PR 1036 - https://github.com/autorope/donkeycar/pull/1036 - mavlink.py includes MavlinkController throttle/steering input and MavlinkDriver for the drivetrain. --- donkeycar/parts/mavlink.py | 266 ++++++++++++++++++++++++++++ donkeycar/templates/cfg_complete.py | 21 ++- donkeycar/templates/complete.py | 8 +- donkeycar/tests/mavlink_test.py | 206 +++++++++++++++++++++ 4 files changed, 496 insertions(+), 5 deletions(-) create mode 100644 donkeycar/parts/mavlink.py create mode 100644 donkeycar/tests/mavlink_test.py diff --git a/donkeycar/parts/mavlink.py b/donkeycar/parts/mavlink.py new file mode 100644 index 000000000..c1218c74e --- /dev/null +++ b/donkeycar/parts/mavlink.py @@ -0,0 +1,266 @@ +#!/usr/bin/env python3 +from __future__ import print_function +""" +Script for using a Pixhawk running any ArduPilot- or PX4-based code with Donkeycar + +author: @zlite (Chris Anderson), 2022 +contrib: @wallarug (Cian Byrne) 2019 +contrib: @peterpanstechland 2019 +contrib: @sctse999 2020 + +If you're getting a permissions error on the USB port: "sudo chmod 666 /dev/ttyACM0" + +requires: +--gcc (sudo apt install gcc) +--pymavlink (pip install pymavlink) +--docopt (pip install docopt) + +""" + +import time +import donkeycar as dk +import config as cfg + +debug = False + +try: + import serial +except ImportError: + print("PySerial not found. Please install: pip install pyserial") + +try: + from pymavlink import mavutil +except ImportError: + print("Pymavlink not found. Please install: pip install pymavlink") + + +class MavlinkController: + ''' + Driver to read MAVLink RC data from a Pixhawk, for manual control + ''' + + def __init__(self, cfg, debug=False): + # standard variables + self.angle = 0.0 + self.throttle = 0.0 + self.mode = 'user' + self.recording = False + self.STEERING_MID = cfg.MAVLINK_STEERING_MID + self.MAX_FORWARD = cfg.MAVLINK_FORWARD + self.STOPPED_PWM = cfg.MAVLINK_STOPPED_PWM + self.MAX_REVERSE = cfg.MAVLINK_MAX_REVERSE + self.SHOW_STEERING_VALUE = cfg.MAVLINK_SHOW_STEERING_VALUE + self.DEAD_ZONE = cfg.JOYSTICK_DEADZONE + self.debug = debug + + master = mavutil.mavlink_connection(cfg.MAVLINK_SERIAL_PORT, baud = 115200) + + def read_serial(self): + ''' + Read the MAVLink values from the serial port connected to the Pixhawk + ''' + # create a mavlink serial instance + msg = master.recv_match() + if not msg: + return + else: + if msg.get_type() == "ATTITUDE": + print ("pitch", round(msg.pitch,2), "roll", round(msg.roll,2), "yaw", round(msg.yaw,2), "pitchspeed", round(msg.pitchspeed,2), "rollspeed", round(msg.rollspeed,2), "yawspeed", round(msg.yawspeed,2)) + if msg.get_type() == "GLOBAL_POSITION_INT": + print ("lat:", msg.lat, "lon:", msg.lon) + if msg.get_type() == "RC_CHANNELS": +# print ("Ch1:", msg.chan1_raw, "Ch2:", msg.chan2_raw, "Ch3:", msg.chan3_raw) + angle_pwm = msg.chan1_raw + throttle_pwm = msg.chan3_raw + if self.debug: + print("angle_pwm = {}, throttle_pwm= {}".format(angle_pwm, throttle_pwm)) + if throttle_pwm >= self.STOPPED_PWM: + # Scale down the input pwm (1500 - 2000) to our max forward + throttle_pwm = dk.utils.map_range_float(throttle_pwm, + 1500, 2000, + self.STOPPED_PWM, + self.MAX_FORWARD ) + # print("remapping throttle_pwm from {} to {}".format(output[1], throttle_pwm)) + + # Go forward + self.throttle = dk.utils.map_range_float(throttle_pwm, + self.STOPPED_PWM, + self.MAX_FORWARD, + 0, 1.0) + else: + throttle_pwm = dk.utils.map_range_float(throttle_pwm, + 1000, 1500, + self.MAX_REVERSE, + self.STOPPED_PWM) + + + # Go backward + self.throttle = dk.utils.map_range_float(throttle_pwm, + self.MAX_REVERSE, + self.STOPPED_PWM, + -1.0, 0) + + if angle_pwm >= self.STEERING_MID: + # Turn Left + self.angle = dk.utils.map_range_float(angle_pwm, + 2000, self.STEERING_MID, + -1, 0) + else: + # Turn Right + self.angle = dk.utils.map_range_float(angle_pwm, + self.STEERING_MID, 1000, + 0, 1) + + if self.debug: + print("angle = {}, throttle = {}".format(self.angle, self.throttle)) + + if self.throttle > self.DEAD_ZONE: + self.recording = True + else: + self.recording = False + + time.sleep(0.01) + + def update(self): + # delay on startup to avoid crashing + print("Warming serial port...") + time.sleep(3) + + while True: + try: + self.read_serial() + except: + print("MAVlink: Error reading serial input!") + break + + def run(self, img_arr=None, mode=None, recording=None): + """ + :param img_arr: current camera image or None + :param mode: default user/mode + :param recording: default recording mode + """ + return self.run_threaded(img_arr, mode, recording) + + def run_threaded(self, img_arr=None, mode=None, recording=None): + """ + :param img_arr: current camera image + :param mode: default user/mode + :param recording: default recording mode + """ + self.img_arr = img_arr + + # + # enforce defaults if they are not none. + # + if mode is not None: + self.mode = mode + if recording is not None: + self.recording = recording + + return self.angle, self.throttle, self.mode, self.recording + + +class MavlinkDriver: + """ + PWM motor controller, for output to motors and servos. + """ + + def __init__(self, cfg, debug=False): + self.MAX_FORWARD = cfg.MAVLINK_MAX_FORWARD + self.MAX_REVERSE = cfg.MAVLINK_MAX_REVERSE + self.STOPPED_PWM = cfg.MAVLINK_STOPPED_PWM + self.STEERING_MID = cfg.MAVLINK_STEERING_MID + self.debug = debug + master = mavutil.mavlink_connection(cfg.MAVLINK_SERIAL_PORT, baud = 115200) + + """ + Steering and throttle should range between -1.0 to 1.0. This function will + trim value great than 1.0 or less than 1.0 + """ + def set_servo_pwm(self, servo_n, microseconds): + """ Sets AUX 'servo_n' output PWM pulse-width. + + Uses https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_SERVO + + 'servo_n' is the AUX port to set (assumes port is configured as a servo). + Valid values are 1-3 in a normal BlueROV2 setup, but can go up to 8 + depending on Pixhawk type and firmware. + 'microseconds' is the PWM pulse-width to set the output to. Commonly + between 1100 and 1900 microseconds. + + """ + # master.set_servo(servo_n+8, microseconds) or: + master.mav.command_long_send( + master.target_system, master.target_component, + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + 0, # first transmission of this command + servo_n + 8, # servo instance, offset by 8 MAIN outputs + microseconds, # PWM pulse-width + 0,0,0,0,0 # unused parameters + ) + def trim_out_of_bound_value(self, value): + if value > 1: + print("MAVLink: Warning, value out of bound. Value = {}".format(value)) + return 1.0 + elif value < -1: + print("MAVLink: Warning, value out of bound. Value = {}".format(value)) + return -1.0 + else: + return value + + def set_pulse(self, steering, throttle): + try: + steering = self.trim_out_of_bound_value(steering) + throttle = self.trim_out_of_bound_value(throttle) + + if throttle > 0: + output_throttle = dk.utils.map_range(throttle, + 0, 1.0, + self.STOPPED_PWM, self.MAX_FORWARD) + else: + output_throttle = dk.utils.map_range(throttle, + -1, 0, + self.MAX_REVERSE, self.STOPPED_PWM) + + if steering > 0: + output_steering = dk.utils.map_range(steering, + 0, 1.0, + self.STEERING_MID, 1000) + else: + output_steering = dk.utils.map_range(steering, + -1, 0, + 2000, self.STEERING_MID) + + if self.is_valid_pwm_value(output_steering) and self.is_valid_pwm_value(output_throttle): + if self.debug: + print("output_steering=%d, output_throttle=%d" % (output_steering, output_throttle)) + self.set_servo_pwm(1, output_steering) + self.set_servo_pwm(2, output_throttle) + self.set_servo_pwm(3, output_throttle) + else: + print(f"Warning: steering = {output_steering}, STEERING_MID = {self.STEERING_MID}") + print(f"Warning: throttle = {output_throttle}, MAX_FORWARD = {self.MAX_FORWARD}, STOPPED_PWM = {self.STOPPED_PWM}, MAX_REVERSE = {self.MAX_REVERSE}") + print("Not sending PWM value to MM1") + + except OSError as err: + print( + "Unexpected issue setting PWM (check wires to motor board): {0}".format(err)) + + def is_valid_pwm_value(self, value): + if 1000 <= value <= 2000: + return True + else: + return False + + def run(self, steering, throttle): + self.set_pulse(steering, throttle) + + def shutdown(self): + try: + self.serial.close() + except: + pass + +if debug: + print ("starting debug") + MavlinkController.read_serial \ No newline at end of file diff --git a/donkeycar/templates/cfg_complete.py b/donkeycar/templates/cfg_complete.py index c9887dab5..395b31066 100644 --- a/donkeycar/templates/cfg_complete.py +++ b/donkeycar/templates/cfg_complete.py @@ -12,7 +12,6 @@ """ - import os #PATHS @@ -57,6 +56,7 @@ # # "PWM_STEERING_THROTTLE" uses two PWM output pins to control a steering servo and an ESC, as in a standard RC car. # "MM1" Robo HAT MM1 board +# "PIXHAWK" Pixhawk (any kind) running ArduPilot or PX4 software. Can control RC, Servo/ESCs, GPS, battery monitoring, wheel encoders, IMU # "SERVO_HBRIDGE_2PIN" Servo for steering and HBridge motor driver in 2pin mode for motor # "SERVO_HBRIDGE_3PIN" Servo for steering and HBridge motor driver in 3pin mode for motor # "DC_STEER_THROTTLE" uses HBridge pwm to control one steering dc motor, and one drive wheel motor @@ -425,7 +425,7 @@ JOYSTICK_MAX_THROTTLE = 0.5 #this scalar is multiplied with the -1 to 1 throttle value to limit the maximum throttle. This can help if you drop the controller or just don't need the full speed available. JOYSTICK_STEERING_SCALE = 1.0 #some people want a steering that is less sensitve. This scalar is multiplied with the steering -1 to 1. It can be negative to reverse dir. AUTO_RECORD_ON_THROTTLE = True #if true, we will record whenever throttle is not zero. if false, you must manually toggle recording with some other trigger. Usually circle button on joystick. -CONTROLLER_TYPE = 'xbox' #(ps3|ps4|xbox|pigpio_rc|nimbus|wiiu|F710|rc3|MM1|custom) custom will run the my_joystick.py controller written by the `donkey createjs` command +CONTROLLER_TYPE = 'xbox' #(ps3|ps4|xbox|pigpio_rc|nimbus|wiiu|F710|rc3|MM1|pixhawk|custom) custom will run the my_joystick.py controller written by the `donkey createjs` command USE_NETWORKED_JS = False #should we listen for remote joystick control over the network? NETWORK_JS_SERVER_IP = None #when listening for network joystick control, which ip is serving this information JOYSTICK_DEADZONE = 0.01 # when non zero, this is the smallest throttle before recording triggered. @@ -461,8 +461,6 @@ PIGPIO_INVERT = False PIGPIO_JITTER = 0.025 # threshold below which no signal is reported - - #ROBOHAT MM1 MM1_STEERING_MID = 1500 # Adjust this value if your car cannot run in a straight line MM1_MAX_FORWARD = 2000 # Max throttle to go fowrward. The bigger the faster @@ -478,6 +476,21 @@ # eg.'/dev/tty.usbmodemXXXXXX' and replace the port accordingly MM1_SERIAL_PORT = '/dev/ttyS0' # Serial Port for reading and sending MM1 data. +#PIXHAWK +MAVLINK_STEERING_MID = 1500 # Adjust this value if your car cannot run in a straight line +MAVLINK_MAX_FORWARD = 2000 # Max throttle to go fowrward. The bigger the faster +MAVLINK_STOPPED_PWM = 1500 +MAVLINK_MAX_REVERSE = 1000 # Max throttle to go reverse. The smaller the faster +MAVLINK_SHOW_STEERING_VALUE = False +# Serial port +# -- Default Pi: '/dev/ttyS0' +# -- Jetson Nano: '/dev/ttyTHS1' +# -- Google coral: '/dev/ttymxc0' +# -- Windows: 'COM3', Arduino: '/dev/ttyACM0' +# -- MacOS/Linux:please use 'ls /dev/tty.*' to find the correct serial port for mm1 +# eg.'/dev/tty.usbmodemXXXXXX' and replace the port accordingly +MAVLINK_SERIAL_PORT = '/dev/ttyS0' # Serial Port for reading and sending MM1 data. + #LOGGING HAVE_CONSOLE_LOGGING = True LOGGING_LEVEL = 'INFO' # (Python logging level) 'NOTSET' / 'DEBUG' / 'INFO' / 'WARNING' / 'ERROR' / 'FATAL' / 'CRITICAL' diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index de01effb9..a63284fda 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -19,7 +19,6 @@ import logging from docopt import docopt - import donkeycar as dk from donkeycar.parts import actuator, pins from donkeycar.parts.transform import TriggeredCallback, DelayedTrigger @@ -247,6 +246,9 @@ def drive(cfg, model_path=None, use_joystick=False, model_type=None, elif cfg.CONTROLLER_TYPE == "MM1": from donkeycar.parts.robohat import RoboHATController ctr = RoboHATController(cfg) + elif cfg.CONTROLLER_TYPE == "pixhawk": + from donkeycar.parts.mavlink import MavlinkController + ctr = MavlinkController(cfg) else: from donkeycar.parts.controller import get_js_controller ctr = get_js_controller(cfg) @@ -747,6 +749,10 @@ def run(self, mode, recording): from donkeycar.parts.robohat import RoboHATDriver V.add(RoboHATDriver(cfg), inputs=['angle', 'throttle']) + elif cfg.DRIVE_TRAIN_TYPE == "PIXHAWK": + from donkeycar.parts.mavlink import MavlinkDriver + V.add(MavlinkDriver(cfg), inputs=['angle', 'throttle']) + elif cfg.DRIVE_TRAIN_TYPE == "PIGPIO_PWM": # # Thi driver is DEPRECATED in favor of 'DRIVE_TRAIN_TYPE == "PWM_STEERING_THROTTLE"' diff --git a/donkeycar/tests/mavlink_test.py b/donkeycar/tests/mavlink_test.py new file mode 100644 index 000000000..6d5ab0818 --- /dev/null +++ b/donkeycar/tests/mavlink_test.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python3 +from __future__ import print_function +""" +Script for using a Pixhawk running any ArduPilot- or PX4-based code with Donkeycar + +author: @zlite (Chris Anderson), 2022 +contrib: @wallarug (Cian Byrne) 2019 +contrib: @peterpanstechland 2019 +contrib: @sctse999 2020 + +If you're getting a permissions error on the USB port: "sudo chmod 666 /dev/ttyACM0" + +MAVlink messages: +read RC PWM: RC_CHANNELS +read attitude: ATTITUDE +read GPS: GLOBAL_POSITION_INT +read battery: BATTERY_STATUS +read encoder: WHEEL_DISTANCE + +write RC PWM: MAV_CMD_DO_SET_SERVO + +""" + +import time +import donkeycar as dk +import config as cfg + +debug = True + +MAX_FORWARD = cfg.MAVLINK_MAX_FORWARD +MAX_REVERSE = cfg.MAVLINK_MAX_REVERSE +STOPPED_PWM = cfg.MAVLINK_STOPPED_PWM +STEERING_MID = cfg.MAVLINK_STEERING_MID +angle = 0.0 +throttle = 0.0 +SHOW_STEERING_VALUE = cfg.MAVLINK_SHOW_STEERING_VALUE +DEAD_ZONE = cfg.JOYSTICK_DEADZONE + + +""" + Steering and throttle should range between -1.0 to 1.0. This function will + trim value great than 1.0 or less than 1.0 +""" + + +try: + import serial +except ImportError: + print("PySerial not found. Please install: pip install pyserial") + +try: + from pymavlink import mavutil +except ImportError: + print("Pymavlink not found. Please install: pip install pymavlink") + +master = mavutil.mavlink_connection(cfg.MAVLINK_SERIAL_PORT, baud = 115200) + +def set_servo_pwm(servo_n, microseconds): + """ Sets AUX 'servo_n' output PWM pulse-width. + + Uses https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_SERVO + + 'servo_n' is the AUX port to set (assumes port is configured as a servo). + Valid values are 1-3 in a normal BlueROV2 setup, but can go up to 8 + depending on Pixhawk type and firmware. + 'microseconds' is the PWM pulse-width to set the output to. Commonly + between 1100 and 1900 microseconds. + + """ + # master.set_servo(servo_n+8, microseconds) or: + master.mav.command_long_send( + master.target_system, master.target_component, + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + 0, # first transmission of this command + servo_n + 8, # servo instance, offset by 8 MAIN outputs + microseconds, # PWM pulse-width + 0,0,0,0,0 # unused parameters + ) + + +def read_serial(): + """ + Read the MAVLink values from the serial port connected to the Pixhawk + """ + # create a mavlink serial instance + + msg = master.recv_match() + if not msg: + return + else: + if msg.get_type() == "ATTITUDE": + print ("pitch", round(msg.pitch,2), "roll", round(msg.roll,2), "yaw", round(msg.yaw,2), "pitchspeed", round(msg.pitchspeed,2), "rollspeed", round(msg.rollspeed,2), "yawspeed", round(msg.yawspeed,2)) + if msg.get_type() == "GLOBAL_POSITION_INT": + print ("lat:", msg.lat, "lon:", msg.lon) + if msg.get_type() == "RC_CHANNELS": + print ("Ch1:", msg.chan1_raw, "Ch2:", msg.chan2_raw, "Ch3:", msg.chan3_raw) + + # angle_pwm = float(output[0]) + # throttle_pwm = float(output[1]) + # if .debug: + # print("angle_pwm = {}, throttle_pwm= {}".format(angle_pwm, throttle_pwm)) + # if throttle_pwm >= .STOPPED_PWM: + # # Scale down the input pwm (1500 - 2000) to our max forward + # throttle_pwm = dk.utils.map_range_float(throttle_pwm, + # 1500, 2000, + # .STOPPED_PWM, + # .MAX_FORWARD ) + # # print("remapping throttle_pwm from {} to {}".format(output[1], throttle_pwm)) + + # # Go forward + # .throttle = dk.utils.map_range_float(throttle_pwm, + # .STOPPED_PWM, + # .MAX_FORWARD, + # 0, 1.0) + # else: + # throttle_pwm = dk.utils.map_range_float(throttle_pwm, + # 1000, 1500, + # .MAX_REVERSE, + # .STOPPED_PWM) + + + # # Go backward + # .throttle = dk.utils.map_range_float(throttle_pwm, + # .MAX_REVERSE, + # .STOPPED_PWM, + # -1.0, 0) + + # if angle_pwm >= .STEERING_MID: + # # Turn Left + # .angle = dk.utils.map_range_float(angle_pwm, + # 2000, .STEERING_MID, + # -1, 0) + # else: + # # Turn Right + # .angle = dk.utils.map_range_float(angle_pwm, + # .STEERING_MID, 1000, + # 0, 1) + + + if throttle > DEAD_ZONE: + recording = True + else: + recording = False + + time.sleep(0.01) + + + +def trim_out_of_bound_value(value): + if value > 1: + print("MAVLink: Warning, value out of bound. Value = {}".format(value)) + return 1.0 + elif value < -1: + print("MAVLink: Warning, value out of bound. Value = {}".format(value)) + return -1.0 + else: + return value + +def set_pulse(steering, throttle): + try: + steering = trim_out_of_bound_value(steering) + throttle = trim_out_of_bound_value(throttle) + + if throttle > 0: + output_throttle = dk.utils.map_range(throttle, + 0, 1.0, + STOPPED_PWM, MAX_FORWARD) + else: + output_throttle = dk.utils.map_range(throttle, + -1, 0, + MAX_REVERSE, STOPPED_PWM) + + if steering > 0: + output_steering = dk.utils.map_range(steering, + 0, 1.0, + STEERING_MID, 1000) + else: + output_steering = dk.utils.map_range(steering, + -1, 0, + 2000, STEERING_MID) + + if is_valid_pwm_value(output_steering) and is_valid_pwm_value(output_throttle): + if debug: + print("output_steering=%d, output_throttle=%d" % (output_steering, output_throttle)) + set_servo_pwm(1, output_steering) + set_servo_pwm(2, output_throttle) + set_servo_pwm(3, output_throttle) + else: + print(f"Warning: steering = {output_steering}, STEERING_MID = {STEERING_MID}") + print(f"Warning: throttle = {output_throttle}, MAX_FORWARD = {MAX_FORWARD}, STOPPED_PWM = {STOPPED_PWM}, MAX_REVERSE = {MAX_REVERSE}") + print("Not sending PWM value to MM1") + + except OSError as err: + print( + "Unexpected issue setting PWM (check wires to motor board): {0}".format(err)) + +def is_valid_pwm_value(value): + if 1000 <= value <= 2000: + return True + else: + return False + +print("starting test") +set_pulse(angle, throttle) +while True: + read_serial() \ No newline at end of file