control.cpp 2.92 KB
Newer Older
markus's avatar
markus committed
1
2
3
4
5
6
7
#include "control.h"

#include <string.h>

Control::Control(Rotor *rotor)
{
  rotor_ = rotor;
markus's avatar
markus committed
8
9
  serial_communicator_ = new SerialCommunicator();
  udp_communicator_ = new UDPCommunicator();
markus's avatar
markus committed
10
11
12
13
14
15
16
17
  debugTime_ = 0;
}

void Control::doCommunicate()
{
  String command_string;
  Communicator::messageError msg_r;

markus's avatar
markus committed
18
19
20
  if((command_string = udp_communicator_->getCommandString(&msg_r)) && (msg_r == Communicator::MSG_OK)) {
    executeCommandString(&command_string, udp_communicator_);
  }
markus's avatar
markus committed
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

  if((command_string = serial_communicator_->getCommandString(&msg_r)) && (msg_r == Communicator::MSG_OK)) {
    executeCommandString(&command_string, serial_communicator_);
  }
}

uint16 Control::setElevation(uint16 el)
{
  return rotor_->setElevation(el);
}

uint16 Control::setAzimuth(uint16 az)
{
  return rotor_->setAzimuth(az);
}

void Control::doControl()
{
  rotor_->doRotor();
  doCommunicate();

  if(debugTime_ > 100)
  {
      //hamlib.debugOut();
markus's avatar
markus committed
45
      //rotor_->debugOut();
markus's avatar
markus committed
46
47
48
49
50
51
52
53
      debugTime_ = 0;
  }
  debugTime_++;
}

Control::hamlibError Control::executeCommandString(String *command_string, Communicator *communicator)
{
    String substring;
54
    String response = "";
markus's avatar
markus committed
55
56
57
    int lastPos = -1;
    int newPos = 0;

58
    while ((newPos = command_string->indexOf(";", lastPos + 1)) != -1) {
markus's avatar
markus committed
59
60
61
62
63
64
        substring = command_string->substring(lastPos + 1, newPos);
        lastPos = newPos;

        if ( substring == "state" )
        {
            // GNAAAARRR
markus's avatar
markus committed
65
            communicator->sendMessage(String(String(AZIM_MIN) + "/" + String(AZIM_MAX) + " " + String(ELEV_MIN) + "/" + String(ELEV_MAX)));
markus's avatar
markus committed
66
        }
markus's avatar
markus committed
67
        else if ( substring == "getpos" ) {
markus's avatar
markus committed
68
            // GNAAAARRR
69
            communicator->sendMessage(String(rotor_->getAzimuth()) + ";" + String(rotor_->getElevation()) + ";");
markus's avatar
markus committed
70
        }
71
72
73
74
75
        else {
            if(executeActiveCommandSubstring(substring) == HAMLIB_OK)
                response += "1";
            else
                response += "0";
markus's avatar
markus committed
76
        }
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
    }

    if(response != "")
        communicator->sendMessage(response);

    return HAMLIB_OK;
}

Control::hamlibError Control::executeActiveCommandSubstring(String substring)
{
    Motor::motorError err;
    float dst = 0.0;
    if (substring.startsWith("setaz") || substring.startsWith("setel")) {
        if( substring.length() < 7 )
            return HAMLIB_NOK;
        dst = substring.substring(5).toFloat();
        if ( dst == 0 && substring.substring(5) != "0" )
            return HAMLIB_NOK;
        if(substring.startsWith("setaz"))
            err = rotor_->setAzimuth(dst);
markus's avatar
markus committed
97
        else
98
99
            err = rotor_->setElevation(dst);
        if(err == Motor::ROT_VAL_OUT_OF_RANGE)
markus's avatar
markus committed
100
101
            return HAMLIB_NOK;
    }
markus's avatar
markus committed
102
103
104
105
106
107
108
109
110
111
    else if (substring == "stop") {
        rotor_->stopAzimuth();
        rotor_->stopElevation();
    }
    else if (substring == "brake") {
        rotor_->stopAndBreakAzimuth();
        rotor_->stopAndBreakElevation();
    }
    else
        return HAMLIB_NOK;
markus's avatar
markus committed
112
113
    return HAMLIB_OK;
}