AIviate/actuator.cpp at master · jsharf/AIviate · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#include "actuator.h"
#include "Servo/Servo.h"
#include <iostream>
#include <time.h>
#include <string>

Servo* servos[ACTUATOR_NUM_AXES];
float backoff_time;
int myDebug;


void usage()
{
    std::cerr << "actuator <in_port> [OPTIONS]" << std::endl;
    std::cerr << "\tOPTIONS:\n" <<
         "\t  -d:\n\t\tEnable debugging\n" <<
         "\t  -b <backoff time>:\n\t\tBackoff time; if multiple command packets are received within the backoff time,\n" <<
         "\t\ttheir values are averaged and applied at the end of that time. (A backoff time of 0 means no backoff.)" <<
         std::endl;
}

int main(int argc, char *argv[])
{
    time_t prev_time;
    backoff_time = 0;
    myDebug = 0;
    if (argc < 2 || argc > 5)
    {
        std::cerr << "Wrong number of arguments" << std::endl;
        usage();
        return 1;
    }
    for(int i = 0; i < argc-1; i++)
    {
        if(strcmp(argv[i+1], "-d") == 0)
        {
            myDebug = 1;
        }

        if(strcmp(argv[i+1], "-b") == 0)
        {
            if(i+2 < argc)
            {
                backoff_time = strtof(argv[i+2], NULL);
            }
            else
            {
                std::cerr << "An argument must be specified for the -b flag." << std::endl;
                usage();
                return 2;
            }
        }
    }

    // Ensure that the first argument is a port number
    long port_number = strtol(argv[1], NULL, 10);
    if(port_number < 0x0001 || port_number > 0xFFFF)
    {
        std::cerr << "First argument must be a port number." << std::endl;
        return 3;

    }

    // Create the UDPListener on the specified port
    UDPListener lst(argv[1]);

    control in_command = control::zero, average_command = control::zero;
    //average_command.ail = average_command.elev = average_command.rudder = average_command.throttle = 0;
    int average_denominator = 0;

    prev_time = time(NULL);

    // Print debug message
    if (myDebug)
    {
        std::cerr << "Outputting debug info on stderr..." << std::endl;
        std::cerr << "Listener start time: " << std::string(ctime(&prev_time)) << std::endl;
    }

    // Construct the servo instances
    for(int i = 0; i < ACTUATOR_NUM_AXES; i++)
    {
        servos[i] = new Servo(i);
    }

    while (true)
    {
        if(lst.receiveControl(in_command) == fail)
        {
            continue;
        }
        if (myDebug)
            std::cerr << in_command << std::endl;
        if(backoff_time > 0)
        {
            if(difftime(time(NULL), prev_time) < backoff_time)
            {
                average_command.ail += in_command.ail;
                average_command.elev += in_command.elev;
                average_command.rudder += in_command.rudder;
                average_command.throttle += in_command.throttle;
                average_denominator++;
                if(myDebug)
                    std::cerr << "Message backed off, added to average queue; size = " << average_denominator << std::endl;
            }
            else
            {
                average_command.ail /= average_denominator;
                average_command.elev /= average_denominator;
                average_command.rudder /= average_denominator;
                average_command.throttle /= average_denominator;

                executeCommand(average_command);

                prev_time = time(NULL);
                average_denominator = 0;
                average_command = control::zero;
            }
        }
        else
        {
            executeCommand(in_command);
        }
    }

    for(int i = 0; i < ACTUATOR_NUM_AXES; i++)
        delete servos[i];
}

bool executeCommand(control &in_command)
{
    if((servos[0]->write((in_command.ail/2) + 0.5)+
            servos[1]->write((in_command.elev/2) + 0.5)+
            servos[2]->write((in_command.rudder/2) + 0.5)+
            servos[3]->write(in_command.throttle)) < 0)
    {
	if(myDebug)
        	std::cerr << "Failed to set control surface positions." << std::endl;
        return false;
    }

    return true;
}