Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : simulator connection for ardupilot version of FlightAxis
17 : */
18 :
19 : #pragma once
20 :
21 : #include <AP_HAL/AP_HAL_Boards.h>
22 : #include "SIM_config.h"
23 :
24 : #if AP_SIM_FLIGHTAXIS_ENABLED
25 :
26 : #include <AP_HAL/utility/Socket_native.h>
27 :
28 : #include "SIM_Aircraft.h"
29 :
30 : namespace SITL {
31 :
32 : /*
33 : a FlightAxis simulator
34 : */
35 : class FlightAxis : public Aircraft {
36 : public:
37 : FlightAxis(const char *frame_str);
38 :
39 : static const struct AP_Param::GroupInfo var_info[];
40 :
41 : /* update model by one time step */
42 : void update(const struct sitl_input &input) override;
43 :
44 : /* static object creator */
45 0 : static Aircraft *create(const char *frame_str) {
46 0 : return NEW_NOTHROW FlightAxis(frame_str);
47 : }
48 :
49 : struct state {
50 : double rcin[12];
51 : double m_airspeed_MPS;
52 : double m_altitudeASL_MTR;
53 : double m_altitudeAGL_MTR;
54 : double m_groundspeed_MPS;
55 : double m_pitchRate_DEGpSEC;
56 : double m_rollRate_DEGpSEC;
57 : double m_yawRate_DEGpSEC;
58 : double m_azimuth_DEG;
59 : double m_inclination_DEG;
60 : double m_roll_DEG;
61 : double m_aircraftPositionX_MTR;
62 : double m_aircraftPositionY_MTR;
63 : double m_velocityWorldU_MPS;
64 : double m_velocityWorldV_MPS;
65 : double m_velocityWorldW_MPS;
66 : double m_velocityBodyU_MPS;
67 : double m_velocityBodyV_MPS;
68 : double m_velocityBodyW_MPS;
69 : double m_accelerationWorldAX_MPS2;
70 : double m_accelerationWorldAY_MPS2;
71 : double m_accelerationWorldAZ_MPS2;
72 : double m_accelerationBodyAX_MPS2;
73 : double m_accelerationBodyAY_MPS2;
74 : double m_accelerationBodyAZ_MPS2;
75 : double m_windX_MPS;
76 : double m_windY_MPS;
77 : double m_windZ_MPS;
78 : double m_propRPM;
79 : double m_heliMainRotorRPM;
80 : double m_batteryVoltage_VOLTS;
81 : double m_batteryCurrentDraw_AMPS;
82 : double m_batteryRemainingCapacity_MAH;
83 : double m_fuelRemaining_OZ;
84 : double m_isLocked;
85 : double m_hasLostComponents;
86 : double m_anEngineIsRunning;
87 : double m_isTouchingGround;
88 : double m_currentAircraftStatus;
89 : double m_currentPhysicsTime_SEC;
90 : double m_currentPhysicsSpeedMultiplier;
91 : double m_orientationQuaternion_X;
92 : double m_orientationQuaternion_Y;
93 : double m_orientationQuaternion_Z;
94 : double m_orientationQuaternion_W;
95 : double m_flightAxisControllerIsActive;
96 : double m_resetButtonHasBeenPressed;
97 : } state, prev_state, next_state;
98 :
99 : static const uint16_t num_keys = sizeof(next_state)/sizeof(double);
100 :
101 : struct keytable {
102 : const char *key;
103 : double &ref;
104 : } keytable[num_keys] = {
105 : { "item", next_state.rcin[0] },
106 : { "item", next_state.rcin[1] },
107 : { "item", next_state.rcin[2] },
108 : { "item", next_state.rcin[3] },
109 : { "item", next_state.rcin[4] },
110 : { "item", next_state.rcin[5] },
111 : { "item", next_state.rcin[6] },
112 : { "item", next_state.rcin[7] },
113 : { "item", next_state.rcin[8] },
114 : { "item", next_state.rcin[9] },
115 : { "item", next_state.rcin[10] },
116 : { "item", next_state.rcin[11] },
117 : { "m-airspeed-MPS", next_state.m_airspeed_MPS },
118 : { "m-altitudeASL-MTR", next_state.m_altitudeASL_MTR },
119 : { "m-altitudeAGL-MTR", next_state.m_altitudeAGL_MTR },
120 : { "m-groundspeed-MPS", next_state.m_groundspeed_MPS },
121 : { "m-pitchRate-DEGpSEC", next_state.m_pitchRate_DEGpSEC },
122 : { "m-rollRate-DEGpSEC", next_state.m_rollRate_DEGpSEC },
123 : { "m-yawRate-DEGpSEC", next_state.m_yawRate_DEGpSEC },
124 : { "m-azimuth-DEG", next_state.m_azimuth_DEG },
125 : { "m-inclination-DEG", next_state.m_inclination_DEG },
126 : { "m-roll-DEG", next_state.m_roll_DEG },
127 : { "m-aircraftPositionX-MTR", next_state.m_aircraftPositionX_MTR },
128 : { "m-aircraftPositionY-MTR", next_state.m_aircraftPositionY_MTR },
129 : { "m-velocityWorldU-MPS", next_state.m_velocityWorldU_MPS },
130 : { "m-velocityWorldV-MPS", next_state.m_velocityWorldV_MPS },
131 : { "m-velocityWorldW-MPS", next_state.m_velocityWorldW_MPS },
132 : { "m-velocityBodyU-MPS", next_state.m_velocityBodyU_MPS },
133 : { "m-velocityBodyV-MPS", next_state.m_velocityBodyV_MPS },
134 : { "m-velocityBodyW-MPS", next_state.m_velocityBodyW_MPS },
135 : { "m-accelerationWorldAX-MPS2", next_state.m_accelerationWorldAX_MPS2 },
136 : { "m-accelerationWorldAY-MPS2", next_state.m_accelerationWorldAY_MPS2 },
137 : { "m-accelerationWorldAZ-MPS2", next_state.m_accelerationWorldAZ_MPS2 },
138 : { "m-accelerationBodyAX-MPS2", next_state.m_accelerationBodyAX_MPS2 },
139 : { "m-accelerationBodyAY-MPS2", next_state.m_accelerationBodyAY_MPS2 },
140 : { "m-accelerationBodyAZ-MPS2", next_state.m_accelerationBodyAZ_MPS2 },
141 : { "m-windX-MPS", next_state.m_windX_MPS },
142 : { "m-windY-MPS", next_state.m_windY_MPS },
143 : { "m-windZ-MPS", next_state.m_windZ_MPS },
144 : { "m-propRPM", next_state.m_propRPM },
145 : { "m-heliMainRotorRPM", next_state.m_heliMainRotorRPM },
146 : { "m-batteryVoltage-VOLTS", next_state.m_batteryVoltage_VOLTS },
147 : { "m-batteryCurrentDraw-AMPS", next_state.m_batteryCurrentDraw_AMPS },
148 : { "m-batteryRemainingCapacity-MAH", next_state.m_batteryRemainingCapacity_MAH },
149 : { "m-fuelRemaining-OZ", next_state.m_fuelRemaining_OZ },
150 : { "m-isLocked", next_state.m_isLocked },
151 : { "m-hasLostComponents", next_state.m_hasLostComponents },
152 : { "m-anEngineIsRunning", next_state.m_anEngineIsRunning },
153 : { "m-isTouchingGround", next_state.m_isTouchingGround },
154 : { "m-currentAircraftStatus", next_state.m_currentAircraftStatus },
155 : { "m-currentPhysicsTime-SEC", next_state.m_currentPhysicsTime_SEC },
156 : { "m-currentPhysicsSpeedMultiplier", next_state.m_currentPhysicsSpeedMultiplier },
157 : { "m-orientationQuaternion-X", next_state.m_orientationQuaternion_X },
158 : { "m-orientationQuaternion-Y", next_state.m_orientationQuaternion_Y },
159 : { "m-orientationQuaternion-Z", next_state.m_orientationQuaternion_Z },
160 : { "m-orientationQuaternion-W", next_state.m_orientationQuaternion_W },
161 : { "m-flightAxisControllerIsActive", next_state.m_flightAxisControllerIsActive },
162 : { "m-resetButtonHasBeenPressed", next_state.m_resetButtonHasBeenPressed },
163 : };
164 :
165 : private:
166 : bool soap_request_start(const char *action, const char *fmt, ...);
167 : char *soap_request_end(uint32_t timeout_ms);
168 : bool exchange_data(const struct sitl_input &input);
169 : void start_controller();
170 : void send_request_message(const struct sitl_input &input);
171 : bool process_reply_message();
172 : void parse_reply(const char *reply);
173 :
174 : bool wait_for_sample(const struct sitl_input &input);
175 : void update_loop(void);
176 : void report_FPS(void);
177 : void socket_creator(void);
178 :
179 : struct state interpolate_frame(struct state& new_state, struct state& old_state, double new_time);
180 :
181 : AP_Int32 _options;
182 : AP_Int16 _samplehz;
183 :
184 : enum class Option : uint32_t{
185 : ResetPosition = (1U<<0),
186 : Rev4Servos = (1U<<1),
187 : HeliDemix = (1U<<2),
188 : SilenceFPS = (1U<<3),
189 : NoDtLog = (1U<<4),
190 : };
191 :
192 : // return true if an option is set
193 0 : bool option_is_set(Option option) const {
194 0 : return (uint32_t(option) & uint32_t(_options)) != 0;
195 : }
196 :
197 : double average_frame_time_s;
198 : double initial_time_s;
199 : double last_time_s;
200 : double last_dt_sample_s;
201 : double last_delta_time_s;
202 : double sample_interval_s;
203 : bool controller_started;
204 : uint32_t glitch_count;
205 : uint64_t frame_counter;
206 : uint64_t activation_frame_counter;
207 : uint64_t socket_frame_counter;
208 : uint64_t last_socket_frame_counter;
209 : double last_frame_count_s;
210 : Vector3d position_offset;
211 : Vector3f last_velocity_ef;
212 :
213 : double next_sample_s;
214 : double average_delta_time_s;
215 :
216 : const char *controller_ip = "127.0.0.1";
217 : uint16_t controller_port = 18083;
218 : SocketAPM_native *socknext;
219 : SocketAPM_native *sock;
220 :
221 : HAL_BinarySemaphore sock_outsem;
222 : HAL_BinarySemaphore sock_insem;
223 :
224 : char replybuf[10000];
225 : pid_t socket_pid;
226 : uint32_t sock_error_count;
227 : double last_recv_sec;
228 : };
229 :
230 :
231 : } // namespace SITL
232 :
233 : #endif // AP_SIM_FLIGHTAXIS_ENABLED
|
