def __init__(self, sb):
print('MultiWiiInterface init')
self.board = MSPy(device="/dev/ttyAMA1",
loglevel='WARNING',
baudrate=115200)
self._loop_dt = 0.05 #0.005
self._get_dt = 0.1 #0.01
self._set_dt = 1 #0.01
super(MultiWiiInterface, self).__init__(sb)
def takeoff_sequence():
takeoff = True
try:
print("Connecting to the FC...")
with MSPy(device=SERIAL_PORT, loglevel='WARNING', baudrate=115200) as board:
if board == 1: # an error occurred...
print("Returned 1, unable to connect to FC.")
return
else:
for msg in command_list:
if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)
print('Sending Disarm command...')
CMDS['aux1'] = 1000
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(1)
# set throttle before arming:
CMDS['throttle'] = 988
# use aux 2 to set autolevel on flight controller
CMDS['aux2'] = 1300
# arming
print('Sending Arm command, waiting 4 seconds')
CMDS['aux1'] = 1800
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(4)
# TODO find good values for throttle on takeoff
# ramp up throttle for takeoff
ascent_throttle = 1600
for i in range(988, ascent_throttle, 5):
CMDS['throttle'] = i
print('throttle up:{}'.format(CMDS['throttle']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.01)
# follow altimeter reading to hover height
hover_throttle = 1500
while takeoff == True:
if altimeter <= 1.5:
CMDS['throttle'] = ascent_throttle
else:
for i in range(ascent_throttle, hover_throttle, -1):
CMDS['throttle'] = i
print('throttle up:{}'.format(CMDS['throttle']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.001)
takeoff = False
# On Linux, your serial port will probably be something like
# /dev/ttyACM0 or /dev/ttyS0 or the same names with numbers different from 0
#
# On Windows, I would expect it to be
# COM1 or COM2 or COM3...
#
# This library uses pyserial, so if you have more questions try to check its docs:
# https://pyserial.readthedocs.io/en/latest/shortintro.html
#
#
serial_port = "/dev/ttyACM0"
# As you run this script, it will save a file MSPy.log with a very detailed info about all the
# things sent and received through the serial port because of the argument loglevel='DEBUG'.
with MSPy(device=serial_port,
logfilename='MSPy.log',
logfilemode='a',
loglevel='DEBUG') as board:
if board == 1:
print(
"An error ocurred... probably the serial port is not available ;)")
sys.exit(1)
#
# The commands bellow will list / test all the possible messages implemented here
# skipping the ones that SET or try to crazy stuff...
for msg in MSPy.MSPCodes.keys():
if 'SET' not in msg:
if msg not in avoid_list:
if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
print('{} >>> requested!'.format(msg))
dataHandler = board.receive_msg()
def keyboard_controller(screen):
CMDS = {
'roll': 1500,
'pitch': 1500,
'throttle': 900,
'yaw': 1500,
'aux1': 1000,
'aux2': 1000
}
# This order is the important bit: it will depend on how your flight controller is configured.
# Below it is considering the flight controller is set to use AETR.
# The names here don't really matter, they just need to match what is used for the CMDS dictionary.
# In the documentation, iNAV uses CH5, CH6, etc while Betaflight goes AUX1, AUX2...
CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2']
# "print" doesn't work with curses, use addstr instead
try:
screen.addstr(15, 0, "Connecting to the FC...")
with MSPy(device=SERIAL_PORT, loglevel='WARNING',
baudrate=115200) as board:
if board == 1: # an error occurred...
return 1
screen.addstr(15, 0, "Connecting to the FC... connected!")
screen.clrtoeol()
screen.move(1, 0)
average_cycle = deque([0] * NO_OF_CYCLES_AVERAGE_GUI_TIME)
# It's necessary to send some messages or the RX failsafe will be activated
# and it will not be possible to arm.
command_list = [
'MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION',
'MSP_BUILD_INFO', 'MSP_BOARD_INFO', 'MSP_UID', 'MSP_ACC_TRIM',
'MSP_NAME', 'MSP_STATUS', 'MSP_STATUS_EX',
'MSP_BATTERY_CONFIG', 'MSP_BATTERY_STATE', 'MSP_BOXNAMES'
]
if board.INAV:
command_list.append('MSPV2_INAV_ANALOG')
command_list.append('MSP_VOLTAGE_METER_CONFIG')
for msg in command_list:
if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)
if board.INAV:
cellCount = board.BATTERY_STATE['cellCount']
else:
cellCount = 0 # MSPV2_INAV_ANALOG is necessary
min_voltage = board.BATTERY_CONFIG['vbatmincellvoltage'] * cellCount
warn_voltage = board.BATTERY_CONFIG[
'vbatwarningcellvoltage'] * cellCount
max_voltage = board.BATTERY_CONFIG['vbatmaxcellvoltage'] * cellCount
screen.addstr(15, 0,
"apiVersion: {}".format(board.CONFIG['apiVersion']))
screen.clrtoeol()
screen.addstr(
15, 50, "flightControllerIdentifier: {}".format(
board.CONFIG['flightControllerIdentifier']))
screen.addstr(
16, 0, "flightControllerVersion: {}".format(
board.CONFIG['flightControllerVersion']))
screen.addstr(
16, 50,
"boardIdentifier: {}".format(board.CONFIG['boardIdentifier']))
screen.addstr(17, 0,
"boardName: {}".format(board.CONFIG['boardName']))
screen.addstr(17, 50, "name: {}".format(board.CONFIG['name']))
slow_msgs = cycle(
['MSP_ANALOG', 'MSP_STATUS_EX', 'MSP_MOTOR', 'MSP_RC'])
cursor_msg = ""
last_loop_time = last_slow_msg_time = last_cycleTime = time.time()
while True:
start_time = time.time()
char = screen.getch() # get keypress
curses.flushinp() # flushes buffer
#
# Key input processing
#
#
# KEYS (NO DELAYS)
#
if char == ord('q') or char == ord('Q'):
break
elif char == ord('d') or char == ord('D'):
cursor_msg = 'Sending Disarm command...'
CMDS['aux1'] = 1000
elif char == ord('r') or char == ord('R'):
screen.addstr(3, 0, 'Sending Reboot command...')
screen.clrtoeol()
board.reboot()
time.sleep(0.5)
break
elif char == ord('a') or char == ord('A'):
cursor_msg = 'Sending Arm command...'
CMDS['aux1'] = 1800
#
# The code below is expecting the drone to have the
# modes set accordingly since everything is hardcoded.
#
elif char == ord('m') or char == ord('M'):
if CMDS['aux2'] <= 1300:
cursor_msg = 'Horizon Mode...'
CMDS['aux2'] = 1500
elif 1700 > CMDS['aux2'] > 1300:
cursor_msg = 'Flip Mode...'
CMDS['aux2'] = 2000
elif CMDS['aux2'] >= 1700:
cursor_msg = 'Angle Mode...'
CMDS['aux2'] = 1000
elif char == ord('w') or char == ord('W'):
CMDS['throttle'] = CMDS['throttle'] + 10 if CMDS[
'throttle'] + 10 <= 2000 else CMDS['throttle']
cursor_msg = 'W Key - throttle(+):{}'.format(
CMDS['throttle'])
elif char == ord('e') or char == ord('E'):
CMDS['throttle'] = CMDS['throttle'] - 10 if CMDS[
'throttle'] - 10 >= 1000 else CMDS['throttle']
cursor_msg = 'E Key - throttle(-):{}'.format(
CMDS['throttle'])
elif char == curses.KEY_RIGHT:
CMDS['roll'] = CMDS['roll'] + 10 if CMDS[
'roll'] + 10 <= 2000 else CMDS['roll']
cursor_msg = 'Right Key - roll(-):{}'.format(CMDS['roll'])
elif char == curses.KEY_LEFT:
CMDS['roll'] = CMDS['roll'] - 10 if CMDS[
'roll'] - 10 >= 1000 else CMDS['roll']
cursor_msg = 'Left Key - roll(+):{}'.format(CMDS['roll'])
elif char == curses.KEY_UP:
CMDS['pitch'] = CMDS['pitch'] + 10 if CMDS[
'pitch'] + 10 <= 2000 else CMDS['pitch']
cursor_msg = 'Up Key - pitch(+):{}'.format(CMDS['pitch'])
elif char == curses.KEY_DOWN:
CMDS['pitch'] = CMDS['pitch'] - 10 if CMDS[
'pitch'] - 10 >= 1000 else CMDS['pitch']
cursor_msg = 'Down Key - pitch(-):{}'.format(CMDS['pitch'])
#
# IMPORTANT MESSAGES (CTRL_LOOP_TIME based)
#
if (time.time() - last_loop_time) >= CTRL_LOOP_TIME:
last_loop_time = time.time()
# Send the RC channel values to the FC
if board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER]):
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)
#
# SLOW MSG processing (user GUI)
#
if (time.time() - last_slow_msg_time) >= SLOW_MSGS_LOOP_TIME:
last_slow_msg_time = time.time()
next_msg = next(slow_msgs) # circular list
# Read info from the FC
if board.send_RAW_msg(MSPy.MSPCodes[next_msg], data=[]):
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)
if next_msg == 'MSP_ANALOG':
voltage = board.ANALOG['voltage']
voltage_msg = ""
if min_voltage < voltage <= warn_voltage:
voltage_msg = "LOW BATT WARNING"
elif voltage <= min_voltage:
voltage_msg = "ULTRA LOW BATT!!!"
elif voltage >= max_voltage:
voltage_msg = "VOLTAGE TOO HIGH"
screen.addstr(
8, 0, "Battery Voltage: {:2.2f}V".format(
board.ANALOG['voltage']))
screen.clrtoeol()
screen.addstr(8, 24, voltage_msg,
curses.A_BOLD + curses.A_BLINK)
screen.clrtoeol()
elif next_msg == 'MSP_STATUS_EX':
ARMED = board.bit_check(board.CONFIG['mode'], 0)
screen.addstr(5, 0, "ARMED: {}".format(ARMED),
curses.A_BOLD)
screen.clrtoeol()
screen.addstr(
5, 50, "armingDisableFlags: {}".format(
board.process_armingDisableFlags(
board.CONFIG['armingDisableFlags'])))
screen.clrtoeol()
screen.addstr(
6, 0,
"cpuload: {}".format(board.CONFIG['cpuload']))
screen.clrtoeol()
screen.addstr(
6, 50,
"cycleTime: {}".format(board.CONFIG['cycleTime']))
screen.clrtoeol()
screen.addstr(7, 0,
"mode: {}".format(board.CONFIG['mode']))
screen.clrtoeol()
screen.addstr(
7, 50, "Flight Mode: {}".format(
board.process_mode(board.CONFIG['mode'])))
screen.clrtoeol()
elif next_msg == 'MSP_MOTOR':
screen.addstr(
9, 0, "Motor Values: {}".format(board.MOTOR_DATA))
screen.clrtoeol()
elif next_msg == 'MSP_RC':
screen.addstr(
10, 0, "RC Channels Values: {}".format(
board.RC['channels']))
screen.clrtoeol()
screen.addstr(
11, 0,
"GUI cycleTime: {0:2.2f}ms (average {1:2.2f}Hz)".
format((last_cycleTime) * 1000,
1 / (sum(average_cycle) / len(average_cycle))))
screen.clrtoeol()
screen.addstr(3, 0, cursor_msg)
screen.clrtoeol()
end_time = time.time()
last_cycleTime = end_time - start_time
if (end_time - start_time) < CTRL_LOOP_TIME:
time.sleep(CTRL_LOOP_TIME - (end_time - start_time))
average_cycle.append(end_time - start_time)
average_cycle.popleft()
finally:
screen.addstr(5, 0, "Disconneced from the FC!")
screen.clrtoeol()
from yamspy import MSPy
#
# On Linux, your serial port will probably be something like
# /dev/ttyACM0 or /dev/ttyS0 or the same names with numbers different from 0
#
# On Windows, I would expect it to be
# COM1 or COM2 or COM3...
#
# This library uses pyserial, so if you have more questions try to check its docs:
# https://pyserial.readthedocs.io/en/latest/shortintro.html
#
#
serial_port = "/dev/ttyAMA1"
with MSPy(device=serial_port, loglevel='DEBUG', baudrate=115200) as board:
# Read info from the FC
# Please, pay attention to the way it works:
# 1. Message is sent: MSP_ALTITUDE without any payload (data=[])
if board.send_RAW_msg(MSPy.MSPCodes['MSP_ALTITUDE'], data=[]):
# 2. Response msg from the flight controller is received
dataHandler = board.receive_msg()
# 3. The msg is parsed
board.process_recv_data(dataHandler)
# 4. After the parser, the instance is populated.
# In this example, SENSOR_DATA has its altitude value updated.
print(board.SENSOR_DATA['altitude'])
# For some msgs there are available specialized methods to read them faster:
# fast_read_altitude
# fast_read_imu
Acknowledgement:
This work was possible thanks to the financial support from IVADO.ca (postdoctoral scholarship 2019/2020).
Disclaimer (adapted from Wikipedia):
None of the authors, contributors, supervisors, administrators, employers, friends, family, vandals, or anyone else
connected (or not) with this project, in any way whatsoever, can be made responsible for your use of the information (code)
contained or linked from here.
"""
import time
from yamspy import MSPy
#
# On Linux, your serial port will probably be something like
# /dev/ttyACM0 or /dev/ttyS0 or the same names with numbers different from 0
#
# On Windows, I would expect it to be
# COM1 or COM2 or COM3...
#
# This library uses pyserial, so if you have more questions try to check its docs:
# https://pyserial.readthedocs.io/en/latest/shortintro.html
#
#
serial_port = "/dev/ttyACM0"
if __name__ == '__main__':
with MSPy(device=serial_port, loglevel='WARNING') as board:
board.reboot() # sometimes it's necessary to reboot your board
# like after a small crash, or the flight controller will not arm again
time.sleep(2)
connected (or not) with this project, in any way whatsoever, can be made responsible for your use of the information (code)
contained or linked from here.
"""
import sys
import time
import serial
from yamspy import MSPy
"""Usage example... and testing ;)
"""
avoid_list = ['MSP_DATAFLASH_ERASE','MSP_DATAFLASH_READ','MSP_EEPROM_WRITE']
with MSPy(device="/dev/ttyACM0") as board:
if board==1:
print("An error ocurred... probably the serial port is not available ;)")
sys.exit(1)
#
# The commands bellow will list / test all the possible messages implemented here
# skipping the ones that SET or try to crazy stuff...
for msg in MSPy.MSPCodes.keys():
if 'SET' not in msg:
if msg not in avoid_list:
if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
print('{} >>> requested!'.format(msg))
dataHandler = board.receive_msg()
codestr = MSPy.MSPCodes2Str.get(dataHandler['code'])
if codestr:
def send_cmds_to_fc(self, pipe_write, pipe_read, CMDS_lock,
barrier_init_values, barrier_sensor_values):
print("send_cmds_to_fc started...")
# (control_imu_pipe_write, control_imu_pipe_read)
prev_time = time.time()
while not self.shutdown:
print("Connecting to the FC...")
with MSPy(device='/dev/ttyS0', loglevel='WARNING',
baudrate=115200) as self.board:
if self.board == 1: # an error occurred...
print(">" * self.NofCHEV +
"Connecting to the FC... FAILED!")
print(">" * self.NofCHEV + "Trying again...")
time.sleep(1)
continue
else:
print(">" * self.NofCHEV + "Connecting to the FC... OK!")
command_list = [
'MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION',
'MSP_BUILD_INFO', 'MSP_BOARD_INFO', 'MSP_UID',
'MSP_ACC_TRIM', 'MSP_NAME', 'MSP_STATUS',
'MSP_STATUS_EX', 'MSP_BATTERY_CONFIG',
'MSP_BATTERY_STATE', 'MSP_BOXNAMES', 'MSP_ANALOG'
]
# Requests the full command_list because we are just starting and most probably not flying / armed.
for msg in command_list:
if self.board.send_RAW_msg(MSPy.MSPCodes[msg],
data=[]):
dataHandler = self.board.receive_msg()
self.board.process_recv_data(dataHandler)
else:
pass # do something in case it fails...
try:
barrier_init_values.wait(
timeout=5
) # indicates the initial values are available
# in a normal situation this should be the last
# member to reach the barrier...
except BrokenBarrierError:
print("send_cmds_to_fc barrier failed...")
self.shutdown = True
local_fast_msp_rc_cmd = self.board.fast_msp_rc_cmd
local_fast_read_imu = self.board.fast_read_imu
local_fast_read_attitude = self.board.fast_read_attitude
local_SENSOR_DATA = self.board.SENSOR_DATA
local_perf_counter = time.perf_counter
local_sleep = time.sleep
tend = local_perf_counter()
try:
while not self.shutdown:
CMDS_RC = []
imu_ready = False
with CMDS_lock:
if self.CMDS:
CMDS_RC = [
self.CMDS[ki] for ki in self.CMDS_ORDER
] # the order must match Betaflight configuration!
if not self.shutdown: # if shutdown is True something crazy may be awaiting in CMDS_RC...
tbegin = local_perf_counter()
tdiff = (tbegin - tend)
if tdiff < self.fc_min_period:
local_sleep(self.fc_min_period - tdiff)
# it may overload the FC if it sends too many commands
if CMDS_RC:
local_fast_msp_rc_cmd(CMDS_RC)
if barrier_sensor_values.n_waiting == 2: # 2=>tof and optflow
local_fast_read_imu(
) # SENSOR_DATA['accelerometer'] and SENSOR_DATA['gyroscope']
barrier_sensor_values.reset(
) # the best way I found... hackish though
imu_ready = True
local_fast_read_attitude(
) # SENSOR_DATA['kinematics'] => roll, pitch, yaw
tend = local_perf_counter()
if not pipe_read.poll():
if imu_ready:
pipe_write.send(
((local_SENSOR_DATA['accelerometer'],
local_SENSOR_DATA['gyroscope'],
local_SENSOR_DATA['kinematics']),
self.mean_batt_voltage, time.time()))
self.frequencies_measurement[
'send_cmds_to_fc'] = time.time() - prev_time
prev_time = time.time()
finally:
print(">" * self.NofCHEV + "SHUTTING DOWN...")
self.shutdown = True
print(">" * self.NofCHEV + "REBOOTING FC...")
self.board.reboot()
print("send_cmds_to_fc closing...")
class MultiWiiInterface(FCInterfaceBase):
board = None
def __init__(self, sb):
print('MultiWiiInterface init')
self.board = MSPy(device="/dev/ttyAMA1",
loglevel='WARNING',
baudrate=115200)
self._loop_dt = 0.05 #0.005
self._get_dt = 0.1 #0.01
self._set_dt = 1 #0.01
super(MultiWiiInterface, self).__init__(sb)
def get(self):
with self.board:
self.board.fast_read_imu()
accelerometer = self.board.SENSOR_DATA['accelerometer']
gyroscope = self.board.SENSOR_DATA['gyroscope']
print(accelerometer)
print(gyroscope)
def set(self):
print('MultiWiiInterface.set placeholder')
# TODO copy this from lilsim, try it on real quad
CMDS = {
'roll': 1500,
'pitch': 1500,
'throttle': 900,
'yaw': 1500,
'aux1': 1000,
'aux2': 1000
}
CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2']
with self.board as board:
# disarm
CMDS['aux1'] = 1000
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
# set throttle
CMDS['throttle'] = 988
# arm
CMDS['aux1'] = 1800
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
# mode
CMDS['aux2'] <= 1300 # Horizon mode
1700 > CMDS['aux2'] > 1300 # Flip Mode
CMDS['aux2'] >= 1700 # Angle Mode
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
# roll
CMDS['roll'] = 1500
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
# pitch
CMDS['pitch'] = 1500
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
# def start(self): -- using base class
# def stop(self): -- using base class
def loop(self):
while True:
with self._lock:
if self._running:
self._loop_last = timelib.time()
if self._loop_last > (self._get_last + self._get_dt):
self._get_last = self._loop_last
self.get()
if self._loop_last > (self._set_last + self._set_dt):
self._set_last = self._loop_last
self.set()
if timelib.time() < (self._loop_last + self._loop_dt):
timelib.sleep(self._loop_last + self._loop_dt -
timelib.time())
else:
print('MultiWiiInterface loop took too long')
else:
print('Exiting MultiWiiInterface loop')
break
def test_thing(queue, testing):
# initial command values
CMDS = {
'roll': 1500,
'pitch': 1500,
'throttle': 900,
'yaw': 1500,
'aux1': 1000,
'aux2': 1000
}
# this order depends on how the flight controller is configured, this order is for AETR
CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2']
# It's necessary to send some messages or the RX failsafe will be activated
# and it will not be possible to arm.
command_list = [
'MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION',
'MSP_BUILD_INFO', 'MSP_BOARD_INFO', 'MSP_UID', 'MSP_ACC_TRIM',
'MSP_NAME', 'MSP_STATUS', 'MSP_STATUS_EX', 'MSP_BATTERY_CONFIG',
'MSP_BATTERY_STATE', 'MSP_BOXNAMES'
]
try:
if testing:
print("testing started, getting commands from queue...")
commands_recv = 0
loop_count = 0
while True:
loop_count += 1
if queue.empty():
print("queue is empty, skipping...")
else:
CMDS = queue.get()
if CMDS == "land":
print("Recieved landing command - landing")
break
else:
CMDS = queue.get()
commands_recv = commands_recv + 1
print("commands received: ", CMDS)
print("commands loop count: ", loop_count)
else:
print("Connecting to the FC...")
with MSPy(device=SERIAL_PORT, loglevel='WARNING',
baudrate=115200) as board:
if board.INAV:
command_list.append('MSPV2_INAV_ANALOG')
command_list.append('MSP_VOLTAGE_METER_CONFIG')
if board == 1: # an error occurred...
print("Returned 1, unable to connect to FC.")
return
else:
for msg in command_list:
if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)
ARMED = board.bit_check(board.CONFIG['mode'], 0)
print("ARMED: {}".format(ARMED))
print("armingDisableFlags: {}".format(
board.process_armingDisableFlags(
board.CONFIG['armingDisableFlags'])))
print("cpuload: {}".format(board.CONFIG['cpuload']))
print("cycleTime: {}".format(board.CONFIG['cycleTime']))
print("mode: {}".format(board.CONFIG['mode']))
print("Flight Mode: {}".format(
board.process_mode(board.CONFIG['mode'])))
# takeoff sequence
# wait for realsense to start
wait_for_realsense = True
while wait_for_realsense:
if queue.empty():
print("queue is empty, waiting...")
else:
wait_for_realsense = False
print("starting takeoff sequence...")
disarmed(board, CMDS, CMDS_ORDER, cycles=200)
armed(board, CMDS, CMDS_ORDER, cycles=300)
throttle_up(board, CMDS, CMDS_ORDER)
hover(board, CMDS, CMDS_ORDER, cycles=200)
print("takeoff sequence complete!")
# fly forward with realsense!
try:
loop_count = 0
while True:
loop_count += 1
if queue.empty():
print("queue is empty, skipping...")
else:
CMDS = queue.get()
if CMDS == "land":
print("Recieved landing command - landing")
throttle_down(board, CMDS, CMDS_ORDER)
armed(board, CMDS, CMDS_ORDER, cycles=100)
disarmed(board, CMDS, CMDS_ORDER, cycles=100)
break
print("commands received: ", CMDS)
print("controls loop count: ", loop_count)
cmd_loop(board, CMDS, CMDS_ORDER)
# hover loop
# disarmed(board, CMDS, CMDS_ORDER, cycles=200)
# armed(board, CMDS, CMDS_ORDER, cycles=300)
# throttle_up(board, CMDS, CMDS_ORDER)
# hover(board, CMDS, CMDS_ORDER, cycles=2000)
# throttle_down(board, CMDS, CMDS_ORDER)
# disarmed(board, CMDS, CMDS_ORDER, cycles=100)
except KeyboardInterrupt:
print("KeyboardInterrupt - landing")
throttle_down(board, CMDS, CMDS_ORDER)
disarmed(board, CMDS, CMDS_ORDER, cycles=100)
finally:
disarmed(board, CMDS, CMDS_ORDER, cycles=100)
print("test over!")
exit(0)
async def send_cmds_to_fc(pipes):
global fc_reboot
global shutdown
global board
global mean_voltage
global controller_battery
print("send_cmds_to_fc started...")
pipe_write, pipe_read = pipes
prev_time = time.time()
while not shutdown:
print("Connecting to the FC...")
with MSPy(device="/dev/ttyACM0", loglevel='WARNING',
baudrate=115200) as board:
if board == 1: # an error occurred...
print("Connecting to the FC... FAILED!")
await asyncio.sleep(1)
continue
else:
print("Connecting to the FC... OK!")
try:
while not shutdown:
if fc_reboot:
shutdown = True
print('REBOOTING...')
break
if controller_battery < 20:
print(
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>PS4 Controller capacity too low !"
)
# shutdown = True
# break
CMDS_RC = [CMDS[ki] for ki in CMDS_ORDER]
# if board.send_RAW_RC(CMDS_RC):
# dataHandler = board.receive_msg()
# board.process_recv_data(dataHandler)
board.fast_msp_rc_cmd(CMDS_RC)
board.fast_read_imu()
board.fast_read_attitude()
if not pipe_read.poll():
# accelerometer = board.SENSOR_DATA['accelerometer']
# gyroscope = board.SENSOR_DATA['gyroscope']
# attitude = board.SENSOR_DATA['kinematics']
pipe_write.send(
((board.SENSOR_DATA['accelerometer'],
board.SENSOR_DATA['gyroscope'],
board.SENSOR_DATA['kinematics']),
mean_voltage, time.time()))
frequencies_measurement['send_cmds_to_fc'] = time.time(
) - prev_time
prev_time = time.time()
await asyncio.sleep(1 / MAIN_FREQ)
finally:
shutdown = True
board.reboot()
print("send_cmds_to_fc closing...")
command_list = [
'MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION', 'MSP_BUILD_INFO',
'MSP_BOARD_INFO', 'MSP_UID', 'MSP_ACC_TRIM', 'MSP_NAME', 'MSP_STATUS',
'MSP_STATUS_EX', 'MSP_BATTERY_CONFIG', 'MSP_BATTERY_STATE', 'MSP_BOXNAMES',
'MSP_ANALOG'
]
CMDS_ORDER = [
'roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2', 'aux3', 'aux4'
]
shutdown = False
fc_reboot = False
try:
while not shutdown:
with MSPy(device="/dev/ttyACM0", loglevel='WARNING',
baudrate=500000) as board:
if board == 1: # an error occurred...
print("Not connected to the FC...")
continue
else:
try:
prev_time = time.time()
while not shutdown:
CMDS_RC = [CMDS[ki] for ki in CMDS_ORDER]
#if board.send_RAW_RC(CMDS_RC):
# dataHandler = board.receive_msg()
# print(dataHandler)
# #board.process_recv_data(dataHandler)
board.fast_msp_rc_cmd(CMDS_RC)
This file is part of YAMSPy.
YAMSPy is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
YAMSPy is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with YAMSPy. If not, see <https://www.gnu.org/licenses/>.
Acknowledgement:
This work was possible thanks to the financial support from IVADO.ca (postdoctoral scholarship 2019/2020).
Disclaimer (adapted from Wikipedia):
None of the authors, contributors, supervisors, administrators, employers, friends, family, vandals, or anyone else
connected (or not) with this project, in any way whatsoever, can be made responsible for your use of the information (code)
contained or linked from here.
"""
import time
from yamspy import MSPy
if __name__ == '__main__':
with MSPy(device="/dev/ttyACM0", loglevel='WARNING') as board:
board.reboot()
time.sleep(2)
def test_autopilot():
try:
print("Connecting to the FC...")
with MSPy(device=SERIAL_PORT, loglevel='WARNING', baudrate=115200) as board:
if board == 1: # an error occurred...
print("Returned 1, unable to connect to FC.")
return
else:
for msg in command_list:
if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
dataHandler = board.receive_msg()
board.process_recv_data(dataHandler)
print('Sending Disarm command...')
CMDS['aux1'] = 1000
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(1)
# print('Sending Reboot command...')
# board.reboot()
# time.sleep(0.5)
# break
# set throttle before arming:
CMDS['throttle'] = 988
print('Sending Arm command, waiting 4 seconds')
CMDS['aux1'] = 1800
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(4)
if CMDS['aux2'] <= 1300:
print('Horizon Mode...')
CMDS['aux2'] = 1500
elif 1700 > CMDS['aux2'] > 1300:
print('Flip Mode...')
CMDS['aux2'] = 2000
elif CMDS['aux2'] >= 1700:
print('Angle Mode...')
CMDS['aux2'] = 1000
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(2)
# test throttling up
for i in range(988, 1500, 5):
CMDS['throttle'] = i
print('throttle up:{}'.format(CMDS['throttle']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.1)
# test roll
CMDS['roll'] = 1500
for i in range(1500, 1800, 5):
CMDS['roll'] = i
print('rolling:{}'.format(CMDS['roll']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.1)
for i in range(1800, 1500, -5):
CMDS['roll'] = i
print('rolling:{}'.format(CMDS['roll']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.1)
# test pitch
CMDS['pitch'] = 1500
for i in range(1500, 1800, 5):
CMDS['pitch'] = i
print('pitching:{}'.format(CMDS['pitch']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.1)
for i in range(1800, 1500, -5):
CMDS['pitch'] = i
print('pitching:{}'.format(CMDS['pitch']))
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(0.1)
# # Send the RC channel values to the FC
# if board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER]):
# dataHandler = board.receive_msg()
# board.process_recv_data(dataHandler)
finally:
print('Sending Disarm command...')
CMDS['aux1'] = 1000
board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER])
time.sleep(2)
print("test over!")
