radiosonde_auto_rx/auto_rx/autorx/rotator.py

#!/usr/bin/env python
#
#   radiosonde_auto_rx - Hamlib Rotator Control
#
#   Copyright (C) 2018  Mark Jessop <vk5qi@rfhead.net>
#   Released under MIT License
#
import logging
import socket
import time
import numpy as np

from threading import Thread, Lock
from autorx.utils import position_info

try:
    # Python 2
    from Queue import Queue
except ImportError:
    # Python 3
    from queue import Queue



def read_rotator(rotctld_host='localhost', rotctld_port=4533, timeout=5):
    ''' Attempt to read a position from a rotctld server.

    Args:
        rotctld_host (str): Hostname of a rotctld instance.
        rotctld_port (int): Port (TCP) of a rotctld instance.

    Returns:
        If successful:
            list: [azimuth, elevation]
        If unsuccessful:
            None
    '''

    try:
        # Initialize the socket.
        _s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        _s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
        _s.settimeout(timeout)

        # Attempt to connect to the rotctld server.
        _s.connect((rotctld_host, rotctld_port))

        # Send position request
        _s.send(b'p\n')

        # Attempt to receive reply.
        _reply = _s.recv(4096)

        # Split reply into lines
        _fields = _reply.decode('ascii').split('\n')
        # Check for an error response, indicated by 'RPRT' in the returned line.
        if 'RPRT' in _fields[0]:
            logging.error("Rotator - rotctld reported error - %s" % _fields[0].strip())
            return None
        else:
            # Attempt to parse the lines as floating point numbers.
            _azimuth = float(_fields[0])
            _elevation = float(_fields[1])

            return [_azimuth, _elevation]

    except Exception as e:
        logging.error("Rotator - Error when reading rotator position - %s" % str(e))
        return None



def set_rotator(rotctld_host='localhost', rotctld_port=4533, azimuth=0.0, elevation = 0.0, timeout=5):
    ''' Attempt to read a position from a rotctld server.

    Args:
        rotctld_host (str): Hostname of a rotctld instance.
        rotctld_port (int): Port (TCP) of a rotctld instance.
        azimuth (float): Target azimuth in degrees True
        elevation (float): Target elevation in degrees.

    Returns:
        If successful:
            True
        If unsuccessful:
            False
    '''

    try:
        # Initialize the socket.
        _s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        _s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
        _s.settimeout(timeout)

        # Attempt to connect to the rotctld server.
        _s.connect((rotctld_host, rotctld_port))

        # Clip the Azimuth and elevation to sane limits.
        _az = azimuth % 360.0
        _el = np.clip(elevation, 0, 90)

        # Send position set command
        _cmd = "P %.1f %.1f\n" % (_az, _el)
        _s.send(_cmd.encode('ascii'))

        # Attempt to receive reply.
        _reply = _s.recv(4096).decode('ascii')

        # Check for an 'OK' response, indicated by 'RPRT 0'
        if 'RPRT 0' in _reply:
            return True
        else:
            # Anything else indicates an error.
            logging.error("Rotator - rotctld reported error: %s" % _reply.strip())
            return False

    except Exception as e:
        logging.error("Rotator - Error when setting rotator position - %s" % str(e))
        return False


class Rotator(object):
    """ Auto-RX Rotator Control Class

    Accepts telemetry dictionaries from a decoder, and turns a rotctld-connected rotator
    to point at the radiosonde.

    """

    # We require the following fields to be present in the input telemetry dict.
    REQUIRED_FIELDS = [ 'id', 'lat', 'lon', 'alt', 'type', 'freq']

    def __init__(self, 
        station_position = [0.0,0.0,0.0],
        rotctld_host = 'localhost',
        rotctld_port = 4533,
        rotator_update_rate = 30,
        rotator_update_threshold = 5.0,
        rotator_homing_enabled = False,
        rotator_homing_delay = 10,
        rotator_home_position = [0.0,0.0]):
        """ Start a new Rotator Control object. 

        Args:
            station_position (list): The location of the receiving station, as a [lat, lon, alt] list.
            
            rotctld_host (str): The hostname where a rotctld instance is running.
            rotctld_port (int): The (TCP) port where a rotctld instance is running.
            
            rotator_update_rate (int): Update the current rotator position to the latest telemetry position every X seconds.
            rotator_update_threshold (float): Only update the rotator position if the new position is at least X degrees
                away from the current position in azimuth or elevation.

            rotator_homing_enabled (bool): If enabled, turn the rotator to a provided azimuth/elevation on startup,
                and whenever no telemetry has been observed for <rotator_homing_delay> minutes.
            rotator_homing_delay (int): Move the rotator to a home position if no telemetry is received within X minutes.
            rotator_home_position (tuple): Rotator home position, as an [azimuth, elevation] list, in degrees (true).

        """

        # Create local copies of the init arguments.
        self.station_position = station_position
        self.rotctld_host = rotctld_host
        self.rotctld_port = rotctld_port
        self.rotator_update_rate = rotator_update_rate
        self.rotator_update_threshold = rotator_update_threshold
        self.rotator_homing_enabled = rotator_homing_enabled
        self.rotator_homing_delay = rotator_homing_delay
        self.rotator_home_position = rotator_home_position


        # Latest telemetry.
        self.latest_telemetry = None
        self.latest_telemetry_time = 0
        self.telem_lock = Lock()

        # Input Queue.
        self.input_queue = Queue()

        # Start queue processing thread.
        self.rotator_thread_running = True
        self.rotator_thread = Thread(target = self.rotator_update_thread)
        self.rotator_thread.start()

        self.log_info("Started Rotator Thread")


    def add(self, telemetry):
        """ Add a telemetery dictionary to the input queue. """
        # Check the telemetry dictionary contains the required fields.
        for _field in self.REQUIRED_FIELDS:
            if _field not in telemetry:
                self.log_error("JSON object missing required field %s" % _field)
                return

        # Update the latest telemetry store.
        self.telem_lock.acquire()
        try:
            self.latest_telemetry = telemetry.copy()
            self.latest_telemetry_time = time.time()
        finally:
            self.telem_lock.release()


    def move_rotator(self, azimuth, elevation):
        ''' Move the rotator to a new position, if the new position
            is further than <rotator_update_threshold> away from the current position
        '''

        # Get current position
        _pos = read_rotator(rotctld_host = self.rotctld_host,
            rotctld_port = self.rotctld_port)

        # If we can't get the current position of the rotator, then we won't be able to move it either
        # May as well return immediately.
        if _pos == None:
            return False

        # Otherwise, compare the current position with the new position.
        # Modulo the azimuth with 360.0, in case we are in an overwind region.
        _curr_az = _pos[0] % 360.0
        _curr_el = _pos[1]

        if (abs(azimuth-_curr_az) > self.rotator_update_threshold) or (abs(elevation-_curr_el) > self.rotator_update_threshold):
            # Move to the target position.
            self.log_info("New rotator target is outside current antenna view (%.1f, %.1f +/- %.1f deg), moving rotator to %.1f, %.1f" % (_curr_az, _curr_el, self.rotator_update_threshold, azimuth, elevation))
            return set_rotator(rotctld_host = self.rotctld_host,
                rotctld_port = self.rotctld_port,
                azimuth = azimuth,
                elevation = elevation)
        else:
            # We are close enough to the target position, no need to move yet.
            self.log_debug("New target is within current antenna view (%.1f, %.1f +/- %.1f deg), not moving rotator." % (_curr_az, _curr_el, self.rotator_update_threshold))
            return True


    def home_rotator(self):
        ''' Move the rotator to it's home position '''
        self.log_info("Moving rotator to home position.")
        self.move_rotator(azimuth = self.rotator_home_position[0],
            elevation = self.rotator_home_position[1])


    def rotator_update_thread(self):
        ''' Rotator updater thread '''

        if self.rotator_homing_enabled:
            # Move rotator to 'home' position on startup.
            self.home_rotator()

        while self.rotator_thread_running:

            _telem = None
            _telem_time = None

            # Grab the latest telemetry data
            self.telem_lock.acquire()
            try:
                if self.latest_telemetry != None:
                    _telem = self.latest_telemetry.copy()
                    _telem_time = self.latest_telemetry_time
            finally:
                self.telem_lock.release()

            # Proceed if we have valid telemetry.
            if _telem != None:
                try:
                    # Check if the telemetry is very old.
                    _telem_age = time.time() -_telem_time

                    # If the telemetry is older than our homing delay, move to our home position.
                    if _telem_age > self.rotator_homing_delay*60.0:
                        self.home_rotator()

                    else:
                        # Otherwise, calculate the new azimuth/elevation.
                        _position = position_info(self.station_position, [_telem['lat'],_telem['lon'],_telem['alt']])

                        # Move to the new position
                        self.move_rotator(_position['bearing'], _position['elevation'])

                except Exception as e:
                    self.log_error("Error handling new telemetry - %s" % str(e))


            # Wait until the next update time.
            _i = 0
            while (_i < self.rotator_update_rate) and self.rotator_thread_running:
                time.sleep(1)
                _i += 1




    def close(self):
        """ Close input processing thread. """
        self.rotator_thread_running = False

        if self.rotator_thread is not None:
            self.rotator_thread.join()

        self.log_debug("Stopped rotator control thread.")


    def running(self):
        """ Check if the logging thread is running.

        Returns:
            bool: True if the logging thread is running.
        """
        return self.rotator_thread_running


    def log_debug(self, line):
        """ Helper function to log a debug message with a descriptive heading. 
        Args:
            line (str): Message to be logged.
        """
        logging.debug("Rotator - %s" % line)


    def log_info(self, line):
        """ Helper function to log an informational message with a descriptive heading. 
        Args:
            line (str): Message to be logged.
        """
        logging.info("Rotator - %s" % line)


    def log_error(self, line):
        """ Helper function to log an error message with a descriptive heading. 
        Args:
            line (str): Message to be logged.
        """
        logging.error("Rotator - %s" % line)


if __name__ == '__main__':
    import sys
    _host = sys.argv[1]

    print(read_rotator(rotctld_host = _host))
    print(set_rotator(rotctld_host = _host, azimuth=0.0, elevation = 0.0))