#!/usr/bin/python3 # -*- coding: utf-8 -*- from subprocess import run, PIPE, Popen import asyncio, time, os, sys, argparse, signal, logging, math, zmq.asyncio, mgrs, traceback, datetime from configobj import ConfigObj from logging.handlers import RotatingFileHandler import numpy as np from geopy import Point from geopy.distance import geodesic,distance,Distance from tinkerforge.ip_connection import IPConnection from tinkerforge.bricklet_imu_v3 import BrickletIMUV3 APP_VERSION = "4.1.5" ### Logging ### rotate_handler = RotatingFileHandler("/bdr/dmcd.log", mode='a', maxBytes=10*1024*1024, backupCount=3, encoding=None, delay=0) log_formatter = logging.Formatter('%(name)s:%(asctime)s - %(levelname)s: %(message)s',"%Y-%m-%d %H:%M:%S") rotate_handler.setFormatter(log_formatter) log = logging.getLogger('DMCd') log.addHandler(rotate_handler) log.propagate = False log.setLevel(logging.INFO) ### Global variables ### FOV = 3 #Field of view in degrees CMDDEBUG=False global sensors_data IMU_UID=None IMU_CALIBRATION_STATUS=None LOGLINELIMIT=200 MILITARY = False TO_NATO_MILS=17.777778 ######################## ### Configuration file ### configfile = '/etc/bdr.conf' # Заменете със своето име на файл conf = ConfigObj(configfile) if str(conf.get('MILITARY', '')).strip().lower() == 'true': MILITARY = True else: MILITARY = False ######################### #MCU cmds SELF_HEADING_DMC = "6500" TARGET_INCLINATION_DMC = "6501" TARGET_GPS = "6104" TARGET_BEARING = "6600" TARGET_DISTANCE = "6601" # SELF_WINDOW_GPS = "6501" # DMC_CALIBRATION_STATUS= "6503" # dirname = os.path.dirname(__file__) tracking = os.path.join(os.path.dirname(__file__), 'tracking.log') websimulator = os.path.join(os.path.dirname(__file__), 'websimulator.py') ### IMU connect ### HOST = "127.0.0.1" PORT = 4223 # UID = "Zkf" #"2161" #"XYZ" ################# def parse_arguments(): parser = argparse.ArgumentParser() parser.add_argument('--debug', action='store_true', help="DEBUG mode.") parser.add_argument('-V', '--version', action='store_true', help='Display the application version.') return parser.parse_args() async def check_conf_file(filename, last_config_load_time): global conf, MILITARY while True: try: last_modified_time = os.path.getmtime(filename) if last_modified_time > last_config_load_time: log.info("CFG Reload...") conf = ConfigObj(filename) MILITARY = conf.get('MILITARY', '').strip().lower() == 'true' last_config_load_time = last_modified_time if MILITARY and all(v is not None for v in [sensors_data.target_gps_lat,sensors_data.target_gps_lon]): target_gps_coords = m.toMGRS(sensors_data.target_gps_lat,sensors_data.target_gps_lon) elif not MILITARY and all(v is not None for v in [sensors_data.target_gps_lat,sensors_data.target_gps_lon]): target_gps_coords = "{:.6f}|{:.6f}".format(sensors_data.target_gps_lat, sensors_data.target_gps_lon) # To MCU cmd_to_mcu(TARGET_GPS,target_gps_coords) if sensors_data.target_distance != None: cmd_to_mcu(TARGET_DISTANCE, str(int(sensors_data.target_distance * 0.91) if conf['LRF'] == 'y' else sensors_data.target_distance)) except Exception as e: log.error("Error checking config file: {}".format(e)) await asyncio.sleep(0.5) def cb_enumerate(uid, connected_uid, position, hardware_version, firmware_version, device_identifier, enumeration_type): global IMU_UID # print("UID: " + uid) if enumeration_type == IPConnection.ENUMERATION_TYPE_DISCONNECTED: print(" Error IMU 3.0") return IMU_UID=uid print("IMU 3.0 UID: {}".format(IMU_UID)) ipcon = IPConnection() # Create IP connection ipcon.connect(HOST, PORT) # Connect to brickd ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE, cb_enumerate) # Register Enumerate Callback ipcon.enumerate() # Trigger Enumerate m = mgrs.MGRS() #ZMQ context = zmq.asyncio.Context() socket = context.socket(zmq.SUB) # type: ignore socket.bind("tcp://*:5555") socket.setsockopt(zmq.SUBSCRIBE, b'LRF') # type: ignore socket.setsockopt(zmq.SUBSCRIBE, b'GPS') # type: ignore time.sleep(1) class sensors_data_class(): def __init__(self): self.lrf_distance = None self.gps_self_lat = None self.gps_self_lon = None self.dmc_heading = None self.dmc_pitch = None self.dmc_roll = None self.dmc_quat_w = None self.dmc_quat_x = None self.dmc_quat_y = None self.dmc_quat_z = None self.dmc_calibrate_status = 3 self.target_gps_lat = None self.target_gps_lon = None self.pre_target_dmc_heading = None self.target_dmc_heading = None self.target_inclination = None self.target_distance = None class CmdTool: def __init__(self, name, cmd): self.name = name self.cmd = cmd def to_arg_list(self, *argv): args = self.cmd.split() for arg in argv: args.append(arg) return args def run(self, *argv): cmd_args = self.to_arg_list(*argv) cmd_out = run(cmd_args, stdout=PIPE, stderr=PIPE, universal_newlines=True) rc = cmd_out.returncode try: output = cmd_out.stdout # int(cmd_out.stdout, base=10) except Exception: output = None return rc, output sensors_data = sensors_data_class() # Callback function for all data callback def cb_all_data(acceleration, magnetic_field, angular_velocity, euler_angle, quaternion, linear_acceleration, gravity_vector, temperature, calibration_status): log.debug(" >>> DMC Calibration: {}".format(calibration_status)) sensors_data.dmc_heading = euler_angle[0]/16.0 sensors_data.dmc_roll = euler_angle[1]/16.0 sensors_data.dmc_pitch = -euler_angle[2]/16.0 sensors_data.dmc_quat_w = quaternion[0]/16383.0 sensors_data.dmc_quat_x = quaternion[1]/16383.0 sensors_data.dmc_quat_y = quaternion[2]/16383.0 sensors_data.dmc_quat_z = quaternion[3]/16383.0 sensors_data.dmc_calibrate_status = calibration_status return sensors_data def cb_orientation(heading, roll, pitch): sensors_data.dmc_heading = int(heading / 16) sensors_data.dmc_roll = int(roll / 16) sensors_data.dmc_pitch = -int(pitch / 16) # sensors_data.dmc_heading = round(heading/16.0) # sensors_data.dmc_roll = round(roll/16.0) # sensors_data.dmc_pitch = round(pitch/16.0) return sensors_data def format_map_url(lat,lon): # OSM #return "http://www.openstreetmap.org/?mlat={}&mlon={}&zoom=15".format(lat,lon) # Gmaps return "https://www.google.com/maps/@{},{},10z".format(lat,lon) def get_bearing(lat1,lon1,lat2,lon2): try: lat1 = float(lat1) lon1 = float(lon1) lat2 = float(lat2) lon2 = float(lon2) except ValueError: # raise ValueError("Missing parameters") pass if not all(isinstance(val, float) for val in [lat1, lon1, lat2, lon2]): # raise ValueError("Missing parameters or not (float)") return dLon = float(lon2 - lon1) y = math.sin(dLon) * math.cos(lat2) x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon) brng = np.rad2deg(math.atan2(y, x)) if brng < 0: brng+= 360 return brng def save_tracklog(): data =[] now = datetime.datetime.now() if not os.path.exists(tracking): with open(tracking, 'w'): pass #Read whole file and enumerate lines # If more than 1000 records are saved with open (tracking,'rb+') as trackingfile: data = trackingfile.read().decode("utf-8").strip().split("\n") if len(data) >= LOGLINELIMIT: data = data[:LOGLINELIMIT] ### Timestamp|BDR Lon,Lat|BDR URL|TRG Lon,Lat|TRG URL|Distance|Azimuth|Inclination ### template = "{0}|{1},{2}|{3}|{4},{5}|{6}|{7}m|{8}°|{9}°" new_line = template.format( now.strftime("%d/%m/%Y %H:%M:%S"), sensors_data.gps_self_lat, sensors_data.gps_self_lon, format_map_url(sensors_data.gps_self_lat, sensors_data.gps_self_lon), sensors_data.target_gps_lat, sensors_data.target_gps_lon, format_map_url(sensors_data.target_gps_lat,sensors_data.target_gps_lon), sensors_data.lrf_distance, sensors_data.dmc_heading, sensors_data.dmc_pitch ) # data[-1] = new_line data.insert(0,new_line) #Should we still reverse order ? #data = data[::-1] with open (tracking,'wb+') as trackingfile: for item in data: trackingfile.write((item+"\n").encode("utf-8")) time.sleep(0.5) Popen(["python3", "targetlog_create.py"]) async def shutdown(sig, loop): try: tasks = [task for task in asyncio.Task.all_tasks() if task is not asyncio.tasks.Task.current_task()] list(map(lambda task: task.cancel(), tasks)) await asyncio.gather(*tasks, return_exceptions=True) # stop the loop loop.stop() except Exception as e: logging.error("shutdown(): {} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e)) async def zmq_loop(loop,handler): global sensors_data try: while True: # try: message = await socket.recv() time.sleep(0.1) data = message.split() if data[0] == b"LRF": log.debug("### LRF zmq data: {} ".format(data[1])) sensors_data.lrf_distance=int(data[1]) await asyncio.ensure_future(handler("LRF")) elif data[0] == b"GPS": log.debug("### GPS zmq data: {} {} ".format(data[1],data[2])) sensors_data.gps_self_lat = float(data[1]) sensors_data.gps_self_lon = float(data[2]) await asyncio.ensure_future(handler("GPS")) except Exception as e: logging.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e)) async def zmq_event_handler(zmq_signal): global sensors_data global MILITARY, conf target_gps_coords = None try: log.debug("Signal {} >>> Heading: {} Incl: {} LRF dist: {} GPS self: {}/{}" .format(zmq_signal, sensors_data.dmc_heading, sensors_data.dmc_pitch, sensors_data.lrf_distance, sensors_data.gps_self_lat, sensors_data.gps_self_lon) ) # if (sensors_data.dmc_heading==0 or sensors_data.dmc_pitch==0): #TODO should we take DMC data here ? heading,roll,pitch=imu.get_orientation() cb_orientation(heading,roll,pitch) ## Triger by LRF if zmq_signal=="LRF": if sensors_data.lrf_distance == 0: sensors_data.pre_target_dmc_heading = sensors_data.dmc_heading if sensors_data.dmc_pitch is not None: sensors_data.target_inclination = sensors_data.dmc_pitch log.info("[PRE_TRG] Azmt {} Inc {}".format(sensors_data.pre_target_dmc_heading,sensors_data.target_inclination)) elif sensors_data.lrf_distance != 0: cmd_to_mcu(TARGET_INCLINATION_DMC,"") cmd_to_mcu(TARGET_DISTANCE,"") cmd_to_mcu(TARGET_BEARING,"") #Zeroing all old target data sensors_data.target_gps_lat=None sensors_data.target_gps_lon=None sensors_data.target_dmc_heading = sensors_data.pre_target_dmc_heading #Set TARGET distance to measured by LRF sensors_data.target_distance=sensors_data.lrf_distance if sensors_data.target_inclination == None: sensors_data.target_inclination = round(sensors_data.dmc_pitch) log.warning("Empyt target_inclination, reinit to {}".format(sensors_data.target_inclination)) cmd_to_mcu(TARGET_INCLINATION_DMC,str(round(sensors_data.target_inclination)) + "°") cmd_to_mcu(TARGET_DISTANCE,str(sensors_data.target_distance)) log.debug("By LRF:{}".format(sensors_data.target_distance)) log.info("[TRG] Azmt {} Inc {}".format(sensors_data.pre_target_dmc_heading,sensors_data.target_inclination)) variables_to_check = [ sensors_data.lrf_distance, sensors_data.dmc_heading, sensors_data.gps_self_lat, sensors_data.gps_self_lon ] if None in variables_to_check: log.debug("*** Missing parameters, not enough data to lock Target ***") else: log.info("Calc TRG by LRF") #Triangualtion of Target distKm=float(sensors_data.lrf_distance/1000) self_gps_point=Point(sensors_data.gps_self_lat, sensors_data.gps_self_lon) bearing=float(sensors_data.target_dmc_heading) #Azimuth log.debug("Self GPS point : {}".format(self_gps_point)) log.debug("Received LRF distance: {}".format(distKm)) log.debug("Azimuth : {}".format(bearing)) #Calculate TARGET geodesic point target_point = geodesic(kilometers=distKm).destination(self_gps_point, bearing).format_decimal() #Cleanup coords sensors_data.target_gps_lat,sensors_data.target_gps_lon = [x.strip() for x in target_point.split(',')] log.debug("===> Calculated target by LRF distance: {}".format(target_point)) #Round up to 6 numbers TARGET GPS coordinates sensors_data.target_gps_lat=round(float(sensors_data.target_gps_lat),6) sensors_data.target_gps_lon=round(float(sensors_data.target_gps_lon),6) target_gps_point = (sensors_data.target_gps_lat, sensors_data.target_gps_lon) sensors_data.target_distance = round(geodesic(self_gps_point, target_gps_point).meters) #get_distance(self_gps_point,target_gps_point) log.debug("TRG dist by GPS: {}".format(sensors_data.target_distance)) target_gps_coords = m.toMGRS(sensors_data.target_gps_lat,sensors_data.target_gps_lon) if MILITARY else "{:.6f}|{:.6f}".format(sensors_data.target_gps_lat, sensors_data.target_gps_lon) # To MCU cmd_to_mcu(TARGET_GPS,target_gps_coords) cmd_to_mcu(TARGET_DISTANCE, str(int(sensors_data.target_distance * 0.91) if conf['LRF'] == 'y' else sensors_data.target_distance)) save_tracklog() #Nulling LRF data after receive and proceed sensors_data.lrf_distance = 0 log.info("By LRF event >>> Target: coords={} distance={}m bearing={}".format(target_gps_coords,sensors_data.target_distance,round(bearing))) ## Trigered by GPS coords elif zmq_signal=="GPS": variables_to_check = [ sensors_data.dmc_heading, sensors_data.gps_self_lat, sensors_data.gps_self_lon, sensors_data.target_gps_lat, sensors_data.target_gps_lon ] if None in variables_to_check: log.debug("Missing parameters, probably not locked Target") else: log.info("Have all data, calculate target by GPS change") target_gps_tuple = (sensors_data.target_gps_lat,sensors_data.target_gps_lon) self_gps_tuple = (sensors_data.gps_self_lat,sensors_data.gps_self_lon) ### Calculation of the Target # Heading by GPS coords gps_bearing = round(get_bearing( sensors_data.target_gps_lat, sensors_data.target_gps_lon, sensors_data.gps_self_lat, sensors_data.gps_self_lon)) log.debug("GPS bearing to target : {}".format(round(gps_bearing))) # Distance by GPS coords sensors_data.target_distance = round(geodesic(self_gps_tuple, target_gps_tuple).meters) log.debug("GPS distance to target: {}".format(sensors_data.target_distance)) # target_gps_coords = str(round(float(sensors_data.target_gps_lat),6))+"|"+str(round(float(sensors_data.target_gps_lon),6)) if MILITARY and all(v is not None for v in [sensors_data.target_gps_lat,sensors_data.target_gps_lon]): target_gps_coords = m.toMGRS(sensors_data.target_gps_lat,sensors_data.target_gps_lon) elif not MILITARY and all(v is not None for v in [sensors_data.target_gps_lat,sensors_data.target_gps_lon]): target_gps_coords = "{:.6f}|{:.6f}".format(sensors_data.target_gps_lat, sensors_data.target_gps_lon) log.info("By GPS event >>> Target: coords={} distance={}m bearing={} MILITARY={}".format(target_gps_coords,sensors_data.target_distance,round(gps_bearing),MILITARY)) # To MCU cmd_to_mcu(TARGET_GPS,target_gps_coords) distance = round(sensors_data.target_distance * 0.91) if 'y' in conf['LRF'] else sensors_data.target_distance cmd_to_mcu(TARGET_DISTANCE, str(distance)) save_tracklog() except Exception as e: log.error("zmq_loop() error: {} {} ".format(traceback.extract_tb(e.__traceback__),e)) def quadrant(angle): angle %= 360.0 if (angle < 0): angle += 360.0; quadrant = int((angle/45) % 8 + 1) quadrantList = ["NE", "E", "SE", "S", "SW", "W", "NW", "N"] return str(quadrantList[quadrant-1]) def cmd_to_mcu(opcode,data): send_buff = [] send_buff.append("E6") send_buff.append("00") send_buff.append(opcode[:2]) send_buff.append(opcode[2:]) for ch in data: #String send_buff.append(hex(ord(ch))[2:]) send_buff.append('00') #ChSum # send_buff[1]=str(hex(len(send_buff))).lower() #Replace lenght byte strlen = len(send_buff) send_buff[1]=str("{0:02x}".format(strlen)) cmd=''.join(str(ch) for ch in send_buff) if CMDDEBUG: print("Lenght {} cmd {}".format(strlen,cmd)) log.debug("Lenght {} cmd {}".format(strlen,cmd)) CmdTool("cmd_to_mcu",websimulator).run(cmd) send_buff.clear() def dmc_event_handler(heading,roll,pitch): global MILITARY, conf try: data_string="" cb_orientation(heading,roll,pitch) if sensors_data.dmc_heading != None: if str(conf['MILITARY']).lower()=='true': #DMC in mils if sensors_data.dmc_calibrate_status == 255: data_string="{}/{}°".format(round(sensors_data.dmc_heading*TO_NATO_MILS), sensors_data.dmc_pitch) else: data_string = "@{}/{}°".format(round(sensors_data.dmc_heading * TO_NATO_MILS), sensors_data.dmc_pitch) else: #DMC in degrees if sensors_data.dmc_calibrate_status == 255: data_string="{}:{:03d}°/{}°".format(quadrant(sensors_data.dmc_heading),sensors_data.dmc_heading, sensors_data.dmc_pitch) else: data_string="@"+("{}:{:03d}°/{}°".format(quadrant(sensors_data.dmc_heading),sensors_data.dmc_heading, sensors_data.dmc_pitch)) cmd_to_mcu(SELF_HEADING_DMC,data_string) deviation_loop() log.info("{:03d}°/{}°".format(sensors_data.dmc_heading, sensors_data.dmc_pitch)) log.debug("Heading: {} Horizont: {} Incl: {}".format(sensors_data.dmc_heading, sensors_data.dmc_roll, sensors_data.dmc_pitch)) except Exception as e: logging.error("dmc_event_handler: {} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e)) def stringify_target_bearing(deviation): global MILITARY pre_dev_bearing="" post_dev_bearing="" if abs(deviation) > FOV: if deviation > 0: pre_dev_bearing = "## " if deviation > 90 else "# " else: post_dev_bearing = " $$" if deviation < -90 else " $" target_dev_bearing = pre_dev_bearing + ("{}°".format(deviation) if not MILITARY else "{}".format(round(deviation*TO_NATO_MILS))) + post_dev_bearing cmd_to_mcu(TARGET_BEARING,target_dev_bearing) log.debug(">>> Target bearing: {}".format(target_dev_bearing)) def deviation_loop(): if all(v is not None for v in [sensors_data.target_gps_lat,sensors_data.target_gps_lon,sensors_data.gps_self_lat,sensors_data.gps_self_lon ]): target_gps_tuple = (sensors_data.target_gps_lat,sensors_data.target_gps_lon) self_gps_tuple = (sensors_data.gps_self_lat,sensors_data.gps_self_lon) # compass_bearing = calculate_initial_compass_bearing(target_gps_tuple,self_gps_tuple) # if DEBUG: log.debug("Compass bearing to target {}".format(compass_bearing)) gps_bearing = get_bearing( sensors_data.target_gps_lat, sensors_data.target_gps_lon, sensors_data.gps_self_lat, sensors_data.gps_self_lon ) log.debug("GPS bearing to target {}".format(gps_bearing)) gps_distance = round(geodesic(self_gps_tuple, target_gps_tuple).meters) log.debug("GPS distance to target {}".format(gps_distance)) # Target distance re-calculate if ( gps_distance != sensors_data.target_distance): log.debug("Diff between measured {} and GPS distance {}m".format(sensors_data.target_distance,gps_distance)) sensors_data.target_distance = gps_distance cmd_to_mcu(TARGET_DISTANCE, str(gps_distance * 0.91 if conf['LRF'] == 'y' else gps_distance)) if all(v is not None for v in [sensors_data.target_dmc_heading, sensors_data.dmc_heading]): deviation_bearing = (int(int(sensors_data.dmc_heading) - int(sensors_data.target_dmc_heading)) + 180) % 360 - 180 log.debug(">>> Target azmth: {} Own azmth: {} Deviation: {}".format(sensors_data.target_dmc_heading,sensors_data.dmc_heading,deviation_bearing)) # Target bearing deviation simbols stringify_target_bearing(deviation_bearing) # Target re-calculate by GPS deviation # stringify_target_bearing(gps_bearing) async def main_loop(loop): try: # Use the current time instead of reading the file's last modification time # start_config_load_timer = time.time() tasks = [ zmq_loop(loop, zmq_event_handler), check_conf_file(configfile, time.time()) ] # Use asyncio.wait() instead of asyncio.gather() # This allows the tasks to run concurrently and returns when all tasks are done await asyncio.wait(tasks) except Exception as e: log.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e)) if __name__ == "__main__": args = parse_arguments() if args.debug: log.setLevel(logging.DEBUG) elif args.version: print("Version: {}".format(APP_VERSION)) sys.exit() try: loop = asyncio.get_event_loop() asyncio.ensure_future(main_loop(loop), loop=loop) imu = BrickletIMUV3(IMU_UID, ipcon) # Create device object imu.register_callback(imu.CALLBACK_ORIENTATION,dmc_event_handler) imu.set_orientation_callback_configuration(500,True) # Interval in ms, on event or not imu.register_callback(imu.CALLBACK_ALL_DATA,cb_all_data) imu.set_all_data_callback_configuration(10000,True) for signame in ('SIGINT', 'SIGTERM'): loop.add_signal_handler(getattr(signal, signame), lambda signame=signame: asyncio.ensure_future(shutdown(signame, loop))) log.info("Version: {} IMU:{}".format(APP_VERSION,IMU_UID)) except Exception as e: logging.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e)) try: loop.run_forever() finally: loop.close()