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!")