def set_pwm_ch3(mavfile): #set_servo doesn't work in mavproxy. Toggle servos doesn't work in Mission Planer either #master.set_servo(args.servo_channel, args.pwm) #Overriding RC command (works in MAVProxy) unused = 0xFFFF #rc_channels_override_send(target_system, target_component, chan_i_raw) unused channels have UInt16_max while not command.end_entered(): pwm_ch3 = raw_input('PWM value (range:1000-2000, safety and test purposes limited 1400-1600) for channel 3 or end/0 : ') if pwm_ch3 == 'end': set_motor_pwm(mavfile, 1500) print('Module override motor ended. Releasing the channel') release_channel(mavfile) break try: pwm_ch3 = int(pwm_ch3) except: print('couldnt handle input') pass if pwm_ch3 >= 1000 and pwm_ch3 <= 2000 and isinstance(pwm_ch3, int): set_motor_pwm(mavfile, pwm_ch3) elif pwm_ch3 == 0: #First 2 parameters are target_system, target_component, last 8 are to release override #mavfile.mav.rc_channels_override_send(0,0,0,0,0,0,0,0,0,0) set_motor_pwm(mavfile, 1500) print('Motor in idle mode: PWM 1500') pass else: print('PWM has to be integer between 1400 and 1550')
def get_param(mavfile): while not command.end_entered(): option = raw_input('Choose an option (1. parameter by ID, 2. parameter by name, 3. all GPS parameters): ') if option == 'end': sys.exit() else: try: option = int(option) except: print('Input has to be an integer from the option list') pass if option == 1: param_id = int(raw_input('Type in ID: ')) name, value, data = receive_msg(mavfile, 'PARAM_VALUE', True, param_id = param_id) print('----------\n' + value[0] + ': ' + str(value[1]) + '\n' + '----------') elif option == 2 or option == '': param_name = raw_input('Type in parameter name: ') inv_dic = {v: k for k, v in dic.items()} if '_' not in param_name: param_name_splitted = param_name.split() param_name = '' for word in param_name_splitted: param_name += word.upper()+'_' param_name = param_name[:-1] #removing the last _ from the string print('Changed input to: ' + param_name) else: pass param_id = int(inv_dic[param_name]) name, value, data = receive_msg(mavfile, 'PARAM_VALUE', True, param_id = param_id) print('----------\n' + value[0] + ': ' + str(value[1]) + '\n' + '----------') elif option == 3: get_pos_param(mavfile, 'print') else: print('Option not defined')
def set_pwm_ch4(mavfile): #set_servo doesn't work in mavproxy. Toggle servos doesn't work in Mission Planer either #master.set_servo(args.servo_channel, args.pwm) #Overriding RC command (works in MAVProxy) unused = 0xFFFF #rc_channels_override_send(target_system, target_component, chan_i_raw) unused channels have UInt16_max while not command.end_entered(): pwm_ch4 = raw_input('PWM value for channel 4 or end : ') #pwm_ch4 = int(pwm_ch4) #mavfile.mav.rc_channels_override_send(mavfile.target_system,mavfile.target_component,unused,unused,unused,pwm_ch4,unused,unused,unused,unused) if pwm_ch4 == 'end': set_servo_pwm(mavfile, 1500) time.sleep(1) print('Releasing channel. Ending now') release_channel(mavfile) break try: pwm_ch4 = int(pwm_ch4) except: print('couldnt handle input') pass if pwm_ch4 >= 1000 and pwm_ch4 <= 20000 and isinstance(pwm_ch4, int): print("OK") #print(mavfile.target_system) #print(mavfile.target_component) #mavfile.mav.rc_channels_override_send(1,1,unused,unused,unused,pwm_ch4,unused,unused,unused,unused) #mavfile.mav.rc_channels_override_send(mavfile.target_system,mavfile.target_component,unused,unused,unused,pwm_ch4,unused,unused,unused,unused) set_servo_pwm(mavfile, pwm_ch4) else: print('PWM has to be integer between 1000 and 2000')
def test_bw(mavfile): # Declaring variables t1 = time.time() t_start = time.time() #----counts ist die Art der Nachricht #counts = {} bytes_recv = 0 bytes_recv2 = 0 secs = 4 try: secs = int(sys.argv[1]) print("For development not waiting for heartbeat") except: #Heartbeat confirmation print("Wait for heartbeat") mavfile.wait_heartbeat() print("Heartbeat received!") rec_prev = 0 rec_prev2 = 0 rec_list =[] print('Sending heartbeat and receiving messages for ' + str(secs) + ' secs') while not command.end_entered(): mavfile.mav.heartbeat_send(1, 1, 1, 1, 1, 1) while mavfile.port.inWaiting() > 0: m = mavfile.recv_msg() #if m == None: break #if m.get_type() not in counts: #counts[m.get_type()] = 0 #counts[m.get_type()] += 1 t_now = time.time() if t_now - t1 > 1: #print(np.floor(t_now-t_start)) print('------------------------------------') print("%u packets received, bwin=%.1f kB/s" % ( mavfile.mav.total_packets_received-rec_prev2, 0.001*(mavfile.mav.total_bytes_received-bytes_recv2)/(t_now-t1))) t1 = t_now; rec_prev2 = mavfile.mav.total_packets_received bytes_recv2 = mavfile.mav.total_bytes_received if t_now - t_start > secs: print('------------------------------------------------------------') print("Total of %u packets received: %.1f packets/s, bwin=%.1f kB/s" % ( mavfile.mav.total_packets_received-rec_prev, (mavfile.mav.total_packets_received-rec_prev)/(t_now - t_start), 0.001*(mavfile.mav.total_bytes_received-bytes_recv)/(t_now - t_start))) break
def autopilot(mavfile): print('Starting') while not command.end_entered(): mode = raw_input('Which mode: (idle, circle, line) ') t_sleep = int(raw_input('Autopilot time: ')) if mode == 'end': break elif mode == 'circle': print('Driving a circle') print('Moving servo') set_servo(mavfile, 2000) sleep(1) print('Starting motor') set_motor(mavfile,1530) sleep(t_sleep) print('Turning off motor') set_motor(mavfile, convert_speed(v = 0)) sleep(1) print('idle') set_servo(mavfile, 1500) release_channel(mavfile) print('done') elif mode == 'line': print('Forward starting') set_motor(mavfile, 1530) sleep(t_sleep) print('Stopping for 2 seconds') set_motor(mavfile, convert_speed(v = 0)) sleep(2) print('Backward starting') set_motor(mavfile, 1470) sleep(t_sleep) print('idle') set_motor(mavfile, convert_speed(v = 0)) release_channel(mavfile) elif mode == 'idle': print('doing nothing') else: print('Mode not supported (yet)')
def print_out_received_data(mavfile, args = False): loop = True; while not command.end_entered() and loop: try: msg_type = sys.argv[1] loop = False except: pass if loop == True: print('----------------------------') list_mav_msg.print_list() msg_type = raw_input('------------------------------------ \nEnter the MAV_MSG type (help/end/return): ') else: pass if '_' not in msg_type and not msg_type.isdigit(): splitted = msg_type.split() name = '' for word in splitted: name += word.upper()+'_' msg_type = name[:-1] #removing the last _ from the string print('Changed input to: ' + msg_type) else: pass id_not_found = False try: msg_type = int(msg_type) ID = msg_type msg_type = list_mav_msg.list_mav_msg_working[str(msg_type)] print('--------------------------------\n ID '+ str(ID) + ' found: ' + msg_type + '\n-------------------------------') except: pass #print(msg_type) #print(type(msg_type)) if msg_type == 'end': sys.exit() elif msg_type == 'help' or msg_type == 'ls': list_mav_msg.print_list() elif msg_type == 'PARAM_VALUE': print('-----------------------\nGenerating list of all Parameters') print('Bad implementation... First go to mavproxy. See how many parameters and hardcode the param_count value') param_count = 403 par_name = [0,]*param_count par_value = [0,]*param_count ls_name = [0,]*param_count ls_id = [0,]*param_count print('Reading out all param_ids') for i in range(0,param_count): #if i == 4: name, value, data = receive_msg(mavfile, 'PARAM_VALUE', True, param_id = i) par_name[i] = value[0] ls_name[i] = value[0] par_value[i] = value[1] ls_id[i] = str( value[4]) print(i) par_data = zip (par_name, par_value) #Used to generate the Dictionary 'param_id_list' print('saving in params_id_list.py') ls = dict(zip(ls_id, ls_name)) #print(ls) txt = open('param_id_list.py', 'w') txt.write('param_dic = {\n') for i in ls_id: txt.write('\''+ str(i)+ '\'' + ' : ' + '\''+ ls[str(i)]+ '\'' +', \n') txt.write('}') txt.close() print('saving in params.py') #for i in par_data: #print(i) txt=open('params.py', 'w') par_data_dic = dict(par_data) for i in par_name: s = '\'' + i + '\'' + ':' + '\'' + str(par_data_dic[i]) +'\'' + ','+ '\n' txt.write(s) txt.close() else: if id_not_found: print('ID not found') break else: pass #plot_str = raw_input ('------------------------------------\n Plot data? (y/n)') plot_str = 'n' if plot_str == 'y': plot = True else: plot = False if loop == True: log_str = raw_input('-------------------------------------\n Log data? (y/n) ') else: log_str = 'n' log = False path = 'log_received_data.txt' if log_str == 'y': log = True cmt_log = raw_input('Add comment: ') cmt = 'Comment: ' +cmt_log + '\n---------------------------------------\n' path_input = raw_input('Add name for log file (Hit Enter for logging into log_received_data.txt: ') if path_input != '': path = '../log/' + path_input + '.csv' else: pass else: log = False #print("Not Receiving messages till terminated") #run_str = "n" if loop == True: run_str = raw_input('--------------------------------------\n Receiving messages till terminated? (y/n): ') else: run_str = 'n' if run_str == 'y': run = True tol_value = 5 tol = True print("no noise tolerance") tol_confirm = "n" #tol_confirm = raw_input('---------------------------------\n Tolerance (noise) in update? (y/n): ') if tol_confirm == 'y': tol = True tol_value = int(raw_input('----------------------------------\n Value of the tolerance: ')) elif tol_confirm == 'n': tol = False else: pass elif run_str == 'n': run = False else: print('Answer not accepted. Only receiving one message') run = False break if loop == True: slicing = "n" #slicing = raw_input('-----------------------------------\n Slicing the list? (y/n) ') else: slicing = "n" if slicing == 'y': start = int(raw_input('Start: ')) -1 end = int(raw_input('End: ')) name, value, data = receive_msg(mavfile, msg_type, True, True, start, end) if 'time' in name[0]: #time_check = raw_input('Print time (for log purposes)? (y/n)') time_check = 'y' if time_check == 'y': time = True #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time, True, start, end) else: time = False #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time, True, start, end) else: time = False pass if log == True: #write_log_number_line(cmt) log_data(name, path) log_data(value, path) else: pass for line in data: print(line) while not command.end_entered() and run: new_name, new_value, new_data = receive_msg(mavfile, msg_type,time, True, start, end) #Only print further data if it differs from the first one if not matching(new_value,value, tol, tol_value): if log == True: log_data(new_value, path) else: pass print('Values changed:') for line in new_data: print(line) value = new_value else: #print(msg_type) #print(type(msg_type)) print('Getting ' + msg_type) sleep(1) time_start = time_now() name, value, data = receive_msg(mavfile, msg_type, True) if 'time' in name[0]: if loop == True: print("Printing time for log purposes") time_check = "y" #time_check = raw_input('--------------------------------\n Print time (for log purposes)? (y/n)') else: time_check = 'n' if time_check == 'y': time = True print('Printing time') #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time) else: time = False print('Not printing time') #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time) else: pass if log == True: #write_log_number_line(cmt) log_data(name, path) log_data(value, path) else: pass for line in data: print(line) while not command.end_entered() and run: new_name, new_value, new_data = receive_msg(mavfile, msg_type, time) #Only print further data if it differs from the first one if not matching(new_value,value, tol, tol_value): if log == True: log_data(new_value, path) else: pass print('Values changed:') for line in new_data: print(line) value= new_value if time_now()-time_start > 120: break
def print_out_received_data(mavfile, args = False): # Calibrate accelerometer if input is -ca try: if sys.argv[1] == '-ca': calibrate_acc(mavfile) sys.exit() else: pass except: pass # Calibrate compass if input is -cc try: if sys.argv[1] == '-cc': calibrate_mag(mavfile) sys.exit() else: pass except: pass # Looping in a while loop loop = True; # Duration while the received data is printed out time_run_duration = 60; while not command.end_entered() and loop: extern = False try: # Extern means that '27' as input from extern was typed in if sys.argv[1] != '-ca' and sys.argv[1] != '-cc': msg_type = sys.argv[1] extern = True else: extern = False pass except: pass # Get a Prompt if no input from extern if loop == True and extern == False: print('----------------------------') list_mav_msg.print_list() msg_type = raw_input('------------------------------------ \nEnter the MAV_MSG type (help/end/return): ') else: pass if '_' not in msg_type and not msg_type.isdigit(): splitted = msg_type.split() name = '' for word in splitted: name += word.upper()+'_' msg_type = name[:-1] #removing the last _ from the string print('Changed input to: ' + msg_type) else: pass id_not_found = False try: msg_type = int(msg_type) ID = msg_type msg_type = list_mav_msg.list_mav_msg_working[str(msg_type)] print('--------------------------------\n ID '+ str(ID) + ' found: ' + msg_type + '\n-------------------------------') except: pass if msg_type == 'end': sys.exit() elif msg_type == 'help' or msg_type == 'ls': list_mav_msg.print_list() elif msg_type == 'PARAM_VALUE': print('-----------------------\nGenerating list of all Parameters') print('Bad implementation... First go to mavproxy. See how many parameters and hardcode the param_count value') # Hard coded amount of parameters. Each ID from 0 to param_count will be saved param_count = 403 par_name = [0,]*param_count par_value = [0,]*param_count ls_name = [0,]*param_count ls_id = [0,]*param_count print('Reading out all param_ids') for i in range(0,param_count): name, value, data = receive_msg(mavfile, 'PARAM_VALUE', True, param_id = i) par_name[i] = value[0] ls_name[i] = value[0] par_value[i] = value[1] ls_id[i] = str( value[4]) print(i) par_data = zip (par_name, par_value) #Used to generate the Dictionary 'param_id_list' print('saving in params_id_list.py') ls = dict(zip(ls_id, ls_name)) txt = open('param_id_list.py', 'w') txt.write('param_dic = {\n') for i in ls_id: txt.write('\''+ str(i)+ '\'' + ' : ' + '\''+ ls[str(i)]+ '\'' +', \n') txt.write('}') txt.close() print('saving in params.py') txt=open('params.py', 'w') par_data_dic = dict(par_data) for i in par_name: s = '\'' + i + '\'' + ':' + '\'' + str(par_data_dic[i]) +'\'' + ','+ '\n' txt.write(s) txt.close() else: if id_not_found: print('ID not found') break else: pass plot_str = 'n' if plot_str == 'y': plot = True else: plot = False if loop == True: log_str = raw_input('-------------------------------------\n Log data? (y/n) ') else: log_str = 'n' log = False path = 'log_received_data.txt' if log_str == 'y': log = True time_run_duration = int(raw_input('Enter the duration of the logging: ')) #cmt_log = raw_input('Add comment: ') cmt_log = '' cmt = 'Comment: ' +cmt_log + '\n---------------------------------------\n' path_input = raw_input('Add name for log file (Hit Enter for logging into log_received_data.txt: ') if path_input == 'gri' or path_input == 'gpi' or path_input == 'all': path_input += get_pos_param(mavfile,'get') else: pass if path_input != '': path = '../log/' + path_input + '.csv' else: pass name_counter = 1 while os.path.isfile(path): path = '../log/' + path_input +'_'+ str(name_counter) +'.csv' else: log = False #print("Not Receiving messages till terminated") #run_str = "n" if loop == True: #run_str = raw_input('--------------------------------------\n Receiving messages till terminated? (y/n): ') run_str= 'y' else: run_str = 'n' if run_str == 'y': run = True tol_value = 5 tol = True print("no noise tolerance") tol_confirm = "n" #tol_confirm = raw_input('---------------------------------\n Tolerance (noise) in update? (y/n): ') if tol_confirm == 'y': tol = True tol_value = int(raw_input('----------------------------------\n Value of the tolerance: ')) elif tol_confirm == 'n': tol = False else: pass elif run_str == 'n': run = False else: print('Answer not accepted. Only receiving one message') run = False break if loop == True: slicing = "n" #slicing = raw_input('-----------------------------------\n Slicing the list? (y/n) ') else: slicing = "n" if slicing == 'y': start = int(raw_input('Start: ')) -1 end = int(raw_input('End: ')) name, value, data = receive_msg(mavfile, msg_type, True, True, start, end) if 'time' in name[0]: #time_check = raw_input('Print time (for log purposes)? (y/n)') time_check = 'y' if time_check == 'y': time = True #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time, True, start, end) else: time = False #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time, True, start, end) else: time = False pass if log == True: #write_log_number_line(cmt) log_data(name, path) log_data(value, path) else: pass for line in data: print(line) while not command.end_entered() and run: new_name, new_value, new_data = receive_msg(mavfile, msg_type,time, True, start, end) #Only print further data if it differs from the first one if not matching(new_value,value, tol, tol_value): if log == True: log_data(new_value, path) else: pass print('Values changed:') for line in new_data: print(line) value = new_value else: if msg_type == 'ALL': path_gpi = path.replace('.csv','_gpi.csv') path_gri = path.replace('.csv','_gri.csv') path_ri = path.replace('.csv','_ri.csv') time_start = time_now() gri_name, gri_value, gri_data = read_gri(mavfile) gpi_name, gpi_value, gpi_data = read_gpi(mavfile) ri_name, ri_value, ri_data = read_ri(mavfile) if log == True: #write_log_number_line(cmt) log_data(gpi_name, path_gpi) log_data(gpi_value, path_gpi) log_data(gri_name, path_gri) log_data(gri_value, path_gri) log_data(ri_name, path_ri) log_data(ri_value, path_ri) else: pass for line in gpi_data: print(line) print('---------------') for line in gri_data: print(line) print('---------------') #for line in ri_data: #print(line) #print('---------------') while not command.end_entered() and run: ngri_name, ngri_value, ngri_data = read_gri(mavfile) ngpi_name, ngpi_value, ngpi_data = read_gpi(mavfile) nri_name, nri_value, nri_data = read_ri(mavfile) #if not matching(new_value,value, tol, tol_value): if log == True: #write_log_number_line(cmt) log_data(ngpi_value, path_gpi) log_data(ngri_value, path_gri) log_data(nri_value, path_ri) else: pass for line in gpi_data: print(line) print('---------------') for line in gri_data: print(line) print('---------------') #for line in ri_data: #print(line) #print('---------------') gri_value = ngri_value gpi_value = ngpi_value ri_value = nri_value if time_now()-time_start > time_run_duration: break else: #print(msg_type) #print(type(msg_type)) print('Getting ' + msg_type) time_start = time_now() name, value, data = receive_msg(mavfile, msg_type, True) if 'lat' and 'lon' in name: lat_idx = name.index('lat') lat_offset = value[lat_idx] lon_idx = name.index('lon') lon_offset = value[lon_idx] offset = True else: offset = False if 'time' in name[0]: if loop == True: #print("Printing time for log purposes") time_check = "y" #time_check = raw_input('--------------------------------\n Print time (for log purposes)? (y/n)') else: time_check = 'n' if time_check == 'y': time = True print('Printing time') #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time) else: time = False print('Not printing time') #Getting the data for the first time name, value, data = receive_msg(mavfile, msg_type, time) else: pass if log == True: #write_log_number_line(cmt) log_data(name, path) log_data(value, path) else: pass for line in data: print(line) print(type(data)) time_compare = time_now() while not command.end_entered() and run: new_name, new_value, new_data = receive_msg(mavfile, msg_type, time) #Only print further data if it differs from the first one if not matching(new_value,value, tol, tol_value) and time_now()-time_compare > 0.5: #if not matching(new_value,value, tol, tol_value): if log == True: log_data(new_value, path) else: pass if offset == True: new_value[lat_idx] = new_value[lat_idx]-lat_offset new_value[lon_idx] = new_value[lon_idx]-lon_offset new_data = zip(new_name, new_value) else: pass print('Values changed:') for line in new_data: print(line) time_compare = time_now() value= new_value if time_now()-time_start > time_run_duration: break
def open_log(mavlink): while not command.end_entered(): chose = 'plot' if chose == 'end' or '': print('Exiting') break elif chose == 'print': select = 'path' print('Log or path? commented out') if select == 'log': number_raw = raw_input('Which log number? (newest or log number) ') txt = open('log_received_data.txt', 'rb') if number_raw == 'end': txt.close() break elif number_raw == 'newest': ls = get_log_list(txt,'newest') print_data(ls[0][1]+3, ls[1][1]) #+3, weil 3 Zeilen Deko else: try: log_number = int(number_raw) except: print('Input invalid') ls = get_log_list(txt,log_number) try: print_data(ls[0]+3,ls[1]-2) #wegen Deko Zahlen dazu und abgezogen except: print('Try \'newest\' instead of ' + str(log_number)) txt.close() elif select == 'path': path = get_log_path() if path == 'end': break else: pass csv_obj = get_column_from_path(path, True) print(csv_obj) else: print('Input not accepted') elif chose == 'plot': path = get_log_path() if path == 'end': break else: pass x_val, y_val = get_column_from_path(path) plt.figure(1) print('Implement title here') plt.title('to be implemented') ts2 = pd.Series(y_val, x_val) plt.axis([min(x_val), max(x_val), min(y_val), max(y_val)]) ts2.plot() plt.show() x_min = min(x_val) x_max = max(x_val) y_min = min(y_val) y_max = max(y_val) print('x_min: ' + str(x_min) + ' x_max: ' + str(x_max) + ' x_diff: ' + str(x_max - x_min)) print('y_min: ' + str(y_min) + ' y_max: ' + str(y_max) + ' y_diff: ' + str(y_max - y_min)) else: print('Chose plot or print (lower case)')
#!/usr/bin/env python import sys, imp, os, glob from command import end_entered #Creating a dictionary for the files path = '/home/pi/asp/pyscripts/modules' os.chdir(path) ls = sorted(glob.glob("cmd*.py")) #vllt spaeter mal rechts noch die Beschreibung laden ls_enum = [0,]*len(ls) for i in range(len(ls)): ls_enum[i] = i+1 dictionary = dict(zip(ls_enum, ls)) for line in dictionary: print(str(line) + ': ' + dictionary[line]) print('__________________________\n') while not end_entered(): #Trying to import a module raw_input('Press Enter') mod = raw_input("Enter a module or 'help'/'end': ") try: mod = int(mod) mod = dictionary[mod] print('Running the module ' + mod) except: pass if mod == 'end' or mod == '': sys.exit() elif mod == 'help' or mod == 'ls': for line in dictionary: print(str(line) + ': ' + dictionary[line]) else: