os.path.abspath(inspect.getfile(inspect.currentframe()))))) import serial.tools.list_ports as list_ports from initialisation import motor_init from main_app import parse_args, read_config import functions.motor_controller_functions as motor_func if __name__ == '__main__': args = parse_args() conf = read_config(args.config_file) ports = list_ports.comports() motor = motor_init(conf['MOTOR'], ports) print("Motor will turn {0} steps per degree of rotation".format( motor['steps_per_degree'])) # Get the current motor pos and if not at HOME move it to HOME motor_deg_pos = motor_func.get_motor_pos(motor['serial']) # in degrees if motor_deg_pos is None: print("No answer from motor. Exit") sys.exit(1) else: print("Motor position: {0}".format(motor_deg_pos)) if motor_deg_pos != motor['home_pos']: t0 = time.perf_counter() moving, motor_step_pos = motor_func.motor_moving( motor['serial'], motor['home_pos'], tolerance=motor['steps_per_degree']) motor_deg_pos = float(motor_step_pos) / motor['steps_per_degree'] print("Homing motor.. {0} --> {1}".format(motor_deg_pos, motor['home_pos']))
def run_one_cycle(counter, conf, db_dict, rad, sample, gps, radiometry_manager, motor, battery, bat_manager, gpios, tpr, rht, trigger_id, verbose): """run one measurement cycle : counter - measurement cycle number, included for logging : conf - main configuration (parsed from file) : sample - main sampling settings configuration : bat_manager - battery manager instance : gps - gps settings dictionary including thread manager instance : radiometry_manager - radiometry manager instance : rad - radiometry configuration : battery - battery management configuration : motor - motor configuration : pgios - gpio pins in use returns: : trigger_id - identifier of the last measurement (a datetime object) """ log = logging.getLogger() # init dicts for all environment checks and latest sensor values ready = { 'speed': False, 'motor': False, 'sun': False, 'rad': False, 'heading': False, 'gps': False } values = { 'speed': None, 'nsat0': None, 'motor_pos': None, 'ship_bearing_mean': None, 'solar_az': None, 'solar_el': None, 'motor_angles': None, 'batt_voltage': None, 'lat0': None, 'lon0': None, 'alt0': None, 'dt': None, 'nsat0': None, 'headMot': None, 'relPosHeading': None, 'accHeading': None, 'fix': None, 'flags_headVehValid': None, 'flags_diffSolN': None, 'flags_gnssFixOK': None, 'tilt_avg': None, 'tilt_std': None, 'inside_temp': None, 'inside_rh': None, 'motor_alarm': None } use_rad = rad['n_sensors'] > 0 # Check whether platform bearing is fixed (set in config) or calculated from GPS if conf['DEFAULT']['use_fixed_bearing'].lower() == 'true': ship_bearing_mean = conf['DEFAULT'].getint('fixed_bearing_deg') bearing_fixed = True else: bearing_fixed = False # Check battery charge - log special messages if required. if battery['used']: if values['batt_voltage'] is None: log.warning("Failed to check Battery Voltage") elif check_battery(bat_manager, battery) == 1: # 0 = OK, 1 = LOW, 2 = CRITICAL message = format_log_message(counter, ready, values) message += "Battery low, idling. Battery info: {1}".format( bat_manager) log.warning(message) return trigger_id # always return last trigger id elif check_battery(bat_manager, battery) == 2: # 0 = OK, 1 = LOW, 2 = CRITICAL message = format_log_message(counter, ready, values) message += "Battery level critical, shutting down. Battery info: {0}".format( bat_manager) log.warning(message) stop_all( db_dict, radiometry_manager, gps_managers, battery, bat_manager, gpios, idle_time=1800 ) # calls sys.exit after pausing for idle_time to prevent immediate restart sys.exit(1) values[ 'batt_voltage'] = bat_manager.batt_voltage # just in case it didn't do that. # Check positioning ready['gps'] = check_gps(gps) ready['heading'] = check_heading(gps) # Check radiometry / sampling conditions ready['rad'] = check_sensors(rad, trigger_id['all_sensors'], radiometry_manager) if ready['gps']: # read latest gps info and calculate angles for motor # valid positioning data is required to do anything else values = update_gps_values(gps, values, tpr, rht) ready['speed'] = check_speed(sample, gps) # read motor position to see if it is ready if motor['used']: ready['motor'], values['motor_alarm'] = check_motor( motor) # check for motor alarms values['motor_pos'] = motor_func.get_motor_pos(motor['serial']) try: values['motor_deg'] = values['motor_pos'] / motor[ 'steps_per_degree'] except: pass if values['motor_pos'] is None: ready['motor'] = False message = format_log_message(counter, ready, values) message += "Motor position not read." log.warning(message) return trigger_id else: # if no motor is used we'll assume the sensors are pointing in the motor home position. values['motor_pos'] = motor['home_pos'] try: values['motor_deg'] = values['motor_pos'] / motor[ 'steps_per_degree'] except: pass # If bearing is not fixed, fetch the calculated mean bearing using data from two GPS sensors if not bearing_fixed: if ready['heading']: values['ship_bearing_mean'] = ( gps['manager'].heading - gps['gps_heading_correction']) % 360.0 else: values['ship_bearing_mean'] = None # collect latest GPS data values = update_gps_values(gps, values) # Fetch sun variables and determine optimal motor pointing angles values['solar_az'], values['solar_el'],\ values['motor_angles'] = azi_func.calculate_positions(values['lat0'], values['lon0'], values['alt0'], values['dt'], values['ship_bearing_mean'], motor, values['motor_pos']) log.debug("[{8}] Sun Az {0:1.0f} | El {1:1.0f} | ViewAz [{2:1.1f}|{3:1.1f}] | MotPos [{4:1.1f}|{5:1.1f}] | MotTarget {6:1.1f} ({7:1.1f})"\ .format(values['solar_az'], values['solar_el'], values['motor_angles']['view_comp_ccw'], values['motor_angles']['view_comp_cw'], values['motor_angles']['ach_mot_ccw'], values['motor_angles']['ach_mot_cw'], values['motor_angles']['target_motor_pos_deg'], values['motor_angles']['target_motor_pos_rel_az_deg'], counter)) # Check if the sun is in a suitable position ready['sun'] = check_sun(sample, values['solar_az'], values['solar_el']) # If the sun is in a suitable position and the motor is not at the required position, move the motor, unless speed criterion is not met # the motor will be moved even if the radiometers are not yet ready to keep them pointed away from the sun if (ready['sun'] and (abs(values['motor_angles']['target_motor_pos_step'] - values['motor_pos']) > motor['step_thresh']))\ and (ready['speed'])\ and (ready['heading'])\ and (motor['used']): log.info("Adjust motor angle ({0} --> {1})".format( values['motor_pos'], values['motor_angles']['target_motor_pos_step'])) # Rotate the motor to the new position target_pos = values['motor_angles']['target_motor_pos_step'] motor_func.rotate_motor(motor_func.commands, target_pos, motor['serial']) moving = True t0 = time.time() # timeout reference while moving and time.time() - t0 < 5: moving, motor_pos = motor_func.motor_moving(motor['serial'], target_pos, tolerance=300) if moving is None: moving = True log.info( "..moving motor.. {0} --> {1} (check again in 2s)".format( motor_pos, target_pos)) if time.time() - t0 > 5: log.warning("Motor movement timed out (this is allowed)") time.sleep(2) ready['ed_sampling'] = check_ed_sampling(use_rad, rad, ready, values) # If all checks are good, take radiometry measurements if all([ use_rad, ready['gps'], ready['rad'], ready['sun'], ready['speed'], ready['heading'] ]): # Get the current time of the computer as a unique trigger id trigger_id['all_sensors'] = datetime.datetime.now() # collect latest GPS and TPR data now that a measurement will be triggered values = update_gps_values(gps, values, tpr, rht) # Collect and combine radiometry data spec_data = [] trig_id, specs, sids, itimes = radiometry_manager.sample_all( trigger_id) for n in range(len(sids)): spec_data.append([str(sids[n]), str(itimes[n]), str(specs[n])]) # If local database is used, commit the data if db_dict['used']: db_id = db_func.commit_db(db_dict, verbose, values, trigger_id['all_sensors'], spectra_data=spec_data, software_version=__version__) log.info("New record (all sensors): {0} [{1}]".format( trigger_id['all_sensors'], db_id)) # If not enough time has passed since the last measurement (rad not ready) and minimum interval to record GPS has not passed, skip to next cycle elif (abs(trigger_id['ed_sensor'].timestamp() - datetime.datetime.now().timestamp()) > rad['ed_sampling_interval'])\ and (ready['ed_sampling']): trigger_id['ed_sensor'] = datetime.datetime.now() values = update_gps_values(gps, values) # collect latest GPS data # trigger Ed spec_data = [] trig_id, specs, sids, itimes = radiometry_manager.sample_ed(trigger_id) for n in range(len(sids)): spec_data.append([str(sids[n]), str(itimes[n]), str(specs[n])]) # If db is used, commit the data to it if db_dict['used']: db_id = db_func.commit_db(db_dict, verbose, values, trigger_id['all_sensors'], spectra_data=spec_data, software_version=__version__) log.info("New record (Ed sensor): {0} [{1}]".format( trigger_id['ed_sensor'], db_id)) else: trigger = False last_any_commit = max([ trigger_id['all_sensors'], trigger_id['ed_sensor'], trigger_id['gps_location'] ]) log.debug("last commit of any kind: {0}".format(last_any_commit)) seconds_elapsed_since_last_any_commit = abs( datetime.datetime.now().timestamp() - last_any_commit.timestamp()) log.debug("seconds since last commit: {0}".format( seconds_elapsed_since_last_any_commit)) if seconds_elapsed_since_last_any_commit > 60: trigger_id['gps_location'] = datetime.datetime.now() # record metadata and GPS data at least every minute values = update_gps_values(gps, values) # collect latest GPS data if db_dict['used']: db_id = db_func.commit_db(db_dict, verbose, values, trigger_id['gps_location'], spectra_data=None, software_version=__version__) log.info("New record (gps location): {0} [{1}]".format( trigger_id['gps_location'], db_id)) message = format_log_message(counter, ready, values) log.info(message) return trigger_id
def run(): """ main program loop. Reads config file to set up environment then starts various threads to monitor inputs. Monitors for a keyboard interrupt to provide a clean exit where possible. """ # Parse the command line arguments for the config file args = parse_args() conf = read_config(args.config_file) # start logging here log = init_logger(conf['LOGGING']) #log = logging.getLogger() log.info('\n===Started logging===\n') try: # Initialise everything db_dict, rad, sample, gps_managers, radiometry_manager,\ motor, battery, bat_manager, gps_checker_manager, gpios = init_all(conf) except Exception: log.exception("Exception during initialisation") stop_all(db_dict, radiometry_manager, gps_managers, gps_checker_manager, battery, bat_manager, gpios) raise log.info("===Initialisation complete===") last_commit_time = datetime.datetime.now() trigger_id = None spec_data = "" ship_bearing_mean = None solar_az = None solar_el = None # Check if the program is using a fixed bearing or calculated one if conf['DEFAULT']['use_fixed_bearing'].lower() == 'true': ship_bearing_mean = conf['DEFAULT'].getint('fixed_bearing_deg') bearing_fixed = True else: bearing_fixed = False checks = {True: "1", False: "0"} # Repeat indefinitely until program closed counter = 0 while True: counter += 1 message = "[{0}] ".format(counter) try: # Check battery charge if battery['used']: if check_battery( bat_manager, battery) == 1: # 0 = OK, 1 = LOW, 2 = CRITICAL message += "Battery low, idling. Battery info: {0}".format( bat_manager) log.info(message) time.sleep(conf['DEFAULT'].getint('main_check_cycle_sec')) continue elif check_battery( bat_manager, battery) == 2: # 0 = OK, 1 = LOW, 2 = CRITICAL message += "Battery level CRITICAL, shutting down. Battery info: {0}".format( bat_manager) log.info(message) stop_all(db_dict, radiometry_manager, gps_managers, gps_checker_manager, battery, bat_manager, gpios, idle_time=1800) else: message += "Bat {0}, ".format(bat_manager.batt_voltage) # reset to default speed_ready = False motor_ready = False sun_suitable = False rad_ready = False # Check if the GPS sensors have met conditions gps_ready = check_gps(gps_managers) message += "GPS {0}, ".format(checks[gps_ready]) # Check if the radiometers have met conditions rad_ready = check_sensors(rad, trigger_id, radiometry_manager) message += "Rad {0}, ".format(checks[rad_ready]) if gps_ready: # read latest gps info and calculate angles for motor speed = gps_managers[0].speed nsat0 = gps_managers[0].satellite_number nsat1 = gps_managers[1].satellite_number speed_ready = check_speed(sample, gps_managers) message += "Speed {0}, ".format(checks[speed_ready]) # Get the current motor pos to check if it's ready motor_pos = motor_func.get_motor_pos(motor['serial']) if motor_pos is None: message += "Motor position not read. NotReady: {0}".format( datetime.datetime.now()) log.info(message) time.sleep(conf['DEFAULT'].getint('main_check_cycle_sec')) continue # If bearing not fixed, fetch the calculated mean bearing using data from two GPS sensors if not bearing_fixed: ship_bearing_mean = gps_checker_manager.mean_bearing lat0 = gps_managers[0].lat lon0 = gps_managers[0].lon alt0 = gps_managers[0].alt dt = gps_managers[0].datetime #dt1 = gps_managers[1].datetime # Fetch sun variables solar_az, solar_el, motor_angles = azi_func.calculate_positions( lat0, lon0, alt0, dt, ship_bearing_mean, motor, motor_pos) log.info("[{8}] Sun Az {0:1.0f} | El {1:1.0f} | ViewAz [{2:1.1f}|{3:1.1f}] | MotPos [{4:1.1f}|{5:1.1f}] | MotTarget {6:1.1f} ({7:1.1f})"\ .format(solar_az, solar_el, motor_angles['view_comp_ccw'], motor_angles['view_comp_cw'], motor_angles['ach_mot_ccw'], motor_angles['ach_mot_cw'], motor_angles['target_motor_pos_deg'], motor_angles['target_motor_pos_rel_az_deg'], counter)) message += "ShBe: {0:1.0f}, SuAz: {1:1.0f}, SuEl: {2:1.1f}. Speed {3:1.1f} nSat [{4}|{5}] "\ .format(ship_bearing_mean, solar_az, solar_el, speed, nsat0, nsat1) # Check if the sun is in a suitable position sun_suitable = check_sun(sample, solar_az, solar_el) # If the sun is in a suitable position and the motor is not at the required position, move the motor, unless speed criterion is not met if (sun_suitable and (abs(motor_angles['target_motor_pos_step'] - motor_pos) > motor['step_thresh'])) and speed_ready: log.info("Adjust motor angle ({0} --> {1})".format( motor_pos, motor_angles['target_motor_pos_step'])) # Rotate the motor to the new position target_pos = motor_angles['target_motor_pos_step'] motor_func.rotate_motor(motor_func.commands, target_pos, motor['serial']) moving = True t0 = time.clock() # timeout reference while moving and time.clock() - t0 < 5: moving, motor_pos = motor_func.motor_moving( motor['serial'], target_pos, tolerance=300) if moving is None: moving = True log.info("..moving motor.. {0} --> {1}".format( motor_pos, target_pos)) time.sleep(2) else: message += "ShBe: None, SuAz: None, SuEl: None, Speed: None. " sun_suitable = False # If all checks are good, take radiometry measurements if all([gps_ready, rad_ready, sun_suitable, speed_ready]): # Get the current time of the computer and data from the GPS sensor managers trigger_id = datetime.datetime.now() gps1_manager_dict = gps_func.create_gps_dict(gps_managers[0]) gps2_manager_dict = gps_func.create_gps_dict(gps_managers[1]) # Collect radiometry data and splice together trig_id, specs, sids, itimes = radiometry_manager.sample_all( trigger_id) spec_data = [] for n in range(len(sids)): spec_data.append( [str(sids[n]), str(itimes[n]), str(specs[n])]) # If db is used, commit the data to it if db_dict['used']: db_id = db_func.commit_db(db_dict, args.verbose, gps1_manager_dict, gps2_manager_dict, trigger_id, ship_bearing_mean, solar_az, solar_el, spec_data) last_commit_time = datetime.datetime.now() message += "Trig: {0} [{1}]".format(trigger_id, db_id) # If not enough time has passed since the last measurement, wait elif abs(datetime.datetime.now().timestamp() - last_commit_time.timestamp()) < 60: # do not execute measurement, do not record metadata message += "Not Ready" else: # record metadata and GPS data if some checks aren't passed gps1_manager_dict = gps_func.create_gps_dict(gps_managers[0]) gps2_manager_dict = gps_func.create_gps_dict(gps_managers[1]) no_trigger_id = datetime.datetime.now() spec_data = None if db_dict['used']: db_id = db_func.commit_db(db_dict, args.verbose, gps1_manager_dict, gps2_manager_dict, no_trigger_id, ship_bearing_mean, solar_az, solar_el, spec_data) last_commit_time = datetime.datetime.now() message += "NotReady | GPS Recorded: {0} [{1}]".format( last_commit_time, db_id) log.info(message) time.sleep(conf['DEFAULT'].getint('main_check_cycle_sec')) except KeyboardInterrupt: log.info("Program interrupted, attempt to close all threads") stop_all(db_dict, radiometry_manager, gps_managers, gps_checker_manager, battery, bat_manager, gpios) except Exception: log.exception("Unhandled Exception") stop_all(db_dict, radiometry_manager, gps_managers, gps_checker_manager, battery, bat_manager, gpios) raise
def init_all(conf): """Initialise all components""" log = logging.getLogger() db = initialisation.db_init(conf['DATABASE']) initialisation.init_gpio(conf, state=0) # set all used pins to LOW # Get all comports and collect the initialisation dicts ports = list_ports.comports() motor = initialisation.motor_init(conf['MOTOR'], ports) gps = initialisation.gps_init(conf['GPS'], ports) rad, Rad_manager = initialisation.rad_init(conf['RADIOMETERS'], ports) sample = initialisation.sample_init(conf['SAMPLING']) battery, bat_manager = initialisation.battery_init(conf['BATTERY'], ports) tpr = initialisation.tpr_init(conf['TPR']) # tilt sensor rht = initialisation.rht_init(conf['RHT']) # internal temp/rh sensor # collect info on which GPIO pins are being used to control peripherals gpios = [] if Rad_manager is not None and rad['use_gpio_control']: gpios.append(rad['gpio1']) # start individual monitoring threads if battery['used']: bat_manager.start() time.sleep(0.1) if gps['manager'] is not None: gps['manager'].add_serial_port(gps['serial1']) gps['manager'].start() if tpr['manager'] is not None: log.info("Starting Tilt/Pitch/Roll manager") tpr['manager'].start() if rht['manager'] is not None: # take a few readings to stabilize for i in range(5): rht_time, rht_rh, rht_temp = rht['manager'].update_rht_single() try: log.info("Internal Temperature and Humidity: {0:2.1f}C {1:2.1f}% ". format(rht_temp, rht_rh)) except: log.error("Could not read RHT sensor") #rht['manager'].start() # there is no need to run an averaging thread, but it's there if we want it if motor['used']: # Get the current motor pos and if not at HOME move it to HOME motor_pos = motor_func.get_motor_pos(motor['serial']) if motor_pos != motor['home_pos']: t0 = time.time() log.info("Homing motor.. {0} --> {1}".format( motor_pos, motor['home_pos'])) motor_func.return_home( motor['serial'] ) # FIXME replace with rotate function to home pos as set in config moving = True while moving and (time.time() - t0 < 5): moving = motor_func.motor_moving(motor['serial'], motor['home_pos'], tolerance=300)[0] if moving is None: moving = True # assume we are not done log.info("..homing motor..") time.sleep(1) log.info("..done") else: log.info("Motor in home position") time.sleep(0.1) # Start the radiometry manager if Rad_manager is not None: log.info("Starting radiometry manager") try: radiometry_manager = Rad_manager(rad) time.sleep(0.1) rad['ed_sampling'] = radiometry_manager.ed_sampling # if the Ed sensor is not identified, disable this feature except Exception as msg: log.exception(msg) stop_all( db, None, gps, battery, bat_manager, gpios, tpr, rht, idle_time=0 ) # calls sys.exit after pausing for idle_time to prevent immediate restart else: radiometry_manager = None # Return all the dicts and manager objects return db, rad, sample, gps, radiometry_manager, motor, battery, bat_manager, gpios, tpr, rht
def init_all(conf): """Initialise all components""" log = logging.getLogger() db = initialisation.db_init(conf['DATABASE']) initialisation.init_gpio(conf, state=0) # set all used pins to LOW # Get all comports and collect the initialisation dicts ports = list_ports.comports() motor = initialisation.motor_init(conf['MOTOR'], ports) gps = initialisation.gps_init(conf['GPS'], ports) rad, Rad_manager = initialisation.rad_init(conf['RADIOMETERS'], ports) sample = initialisation.sample_init(conf['SAMPLING']) battery, bat_manager = initialisation.battery_init(conf['BATTERY'], ports) # start the battery manager if battery['used']: bat_manager.start() time.sleep(0.1) # collect info on which GPIO pins are being used gpios = [] if gps['gpio_control']: gpios.append(gps['gpio2']) if rad['use_gpio_control']: gpios.append(rad['gpio1']) gpios.append(rad['gpio2']) gpios.append(rad['gpio3']) # Get the GPS serial objects from the GPS dict gps_ports = [port for key, port in gps.items() if 'serial' in key] # Instantiate GPS monitoring threads if len(gps_ports) > 0: gps_managers = [] for port in gps_ports: gps_manager = GPSManager() gps_manager.add_serial_port(port) gps_manager.start() gps_managers.append(gps_manager) else: log.info("Check GPS sensors and Motor connection settings") time.sleep(0.1) # Start the GPS checker thread gps_checker_manager = GPSChecker(gps_managers) gps_checker_manager.start() # Get the current motor pos and if not at HOME move it to HOME motor_pos = motor_func.get_motor_pos(motor['serial']) if motor_pos != motor['home_pos']: t0 = time.clock() log.info("Homing motor.. {0} --> {1}".format(motor_pos, motor['home_pos'])) motor_func.return_home( motor['serial'] ) # FIXME replace with rotate function to home pos as set in config moving = True while moving and (time.clock() - t0 < 5): moving = motor_func.motor_moving(motor['serial'], motor['home_pos'], tolerance=300)[0] if moving is None: moving = True # assume we are not done log.info("..homing motor..") time.sleep(1) log.info("..done") else: log.info("Motor in home position") time.sleep(0.1) # Start the radiometry manager log.info("Starting radiometry manager") radiometry_manager = Rad_manager(rad) time.sleep(0.1) # Return all the dicts and manager objects return db, rad, sample, gps_managers, radiometry_manager, motor, battery, bat_manager, gps_checker_manager, gpios