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