Repository navigation
Expand file tree
/
Copy pathMicroDronInterfaceUDP.h
More file actions
142 lines (88 loc) · 3.57 KB
/
Copy pathMicroDronInterfaceUDP.h
File metadata and controls
142 lines (88 loc) · 3.57 KB
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
//
// Created by abiel on 8/19/20.
//
#ifndef MICRODRONITESM_GUI_MICRODRONINTERFACEUDP_H
#define MICRODRONITESM_GUI_MICRODRONINTERFACEUDP_H
#include "MicroDronInterface.h"
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <cstdlib>
#include <fcntl.h>
#include <arpa/inet.h>
#include <cstring>
#include <thread>
#include "mavlink.h"
#include <atomic>
#include <map>
#include "ESPComms.h"
struct MotorValues{
float frontLeft, frontRight;
float backLeft, backRight;
};
extern "C"{
#include <UDP.h>
};
class MicroDronInterfaceUDP : public MicroDronInterface {
public:
MicroDronInterfaceUDP();
SimplePID getPitchPid() const override;
SimplePID getRollPid() const override;
SimplePID getYawPid() const override;
SimplePID getHeightPid() const override;
void setPitchPid(SimplePID pitchPid) override;
void setRollPid(SimplePID rollPid) override;
void setYawPid(SimplePID yawPid) override;
void setHeightPid(SimplePID heightPid) override;
void sendHeartBeat() override;
std::chrono::high_resolution_clock::time_point getLastAttUpdateTime() const;
float getPitch() const override;
float getRoll() const override;
float getYaw() const override;
float getHeight() const override;
int getMode() const override;
float getK() const override;
float getMotorOutput1() const override;
float getMotorOutput2() const override;
float getMotorOutput3() const override;
float getMotorOutput4() const override;
void setAllMotorOutput(float output) override;
void setAllMotorOutput(float output1, float output2, float output3, float output4) override;
void setManualSetpoint(float roll, float pitch, float yaw, float thrust, bool applyFeedForward = false) override;
void sendJoystickControl(int16_t x, int16_t y, int16_t z, int16_t r) override;
void setK(float newK) override;
void emergencyStop(bool state) override;
bool isEmergencyStopped() const override;
bool isConnected() const override;
float getHeartbeatTime() const override;
MotorValues getMotorValues() const;
std::chrono::high_resolution_clock::time_point getLastMotorUpdate() const;
void requestParamList();
std::map<int, mavlink_param_value_t>& getParams();
void setParameter(const std::string &name, float value);
void setParameter(const mavlink_param_set_t ¶mSet);
~MicroDronInterfaceUDP();
private:
void update();
void hbUpdate();
void sendMessage(const mavlink_message_t &msg);
void sendAndCheckParams();
bool emergencyStopped{false};
const uint16_t gs_port = 14550;
struct sockaddr_in gs_server{}, gs_client{}; //Local IP addr
std::atomic<mavlink_attitude_t> attitude{};
std::atomic<mavlink_distance_sensor_t> distanceSensor{};
std::chrono::high_resolution_clock::time_point lastHb;
std::map<int, mavlink_param_value_t> paramList;
std::thread updateThread, hbThread, paramsThread;
std::mutex updateMutex, nextToAccessMutex, lowPriorityMutex;
std::map<std::string, mavlink_param_set_t> pendingParams {};
bool isRunning = true;
MotorValues motorValues;
std::chrono::high_resolution_clock::time_point lastMotorUpdateTime = std::chrono::high_resolution_clock::now();
std::chrono::high_resolution_clock::time_point lastAttUpdateTime = std::chrono::high_resolution_clock::now();
std::chrono::high_resolution_clock::time_point systemStart = std::chrono::high_resolution_clock::now();
double timeSinceStart = 0.0; //Seconds
ESPComms *comms;
};
#endif //MICRODRONITESM_GUI_MICRODRONINTERFACEUDP_H