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 : gimbal simulator class for MAVLink gimbal
17 : */
18 :
19 : #include "SIM_SoloGimbal.h"
20 :
21 : #if AP_SIM_SOLOGIMBAL_ENABLED
22 :
23 : #include <stdio.h>
24 :
25 : #include "SIM_Aircraft.h"
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : #define GIMBAL_DEBUG 0
30 :
31 : #if GIMBAL_DEBUG
32 : #define debug(fmt, args...) do { printf("GIMBAL: " fmt, ##args); } while(0)
33 : #else
34 : #define debug(fmt, args...) do { } while(0)
35 : #endif
36 :
37 : namespace SITL {
38 :
39 : /*
40 : update the gimbal state
41 : */
42 248542 : void SoloGimbal::update(const Aircraft &aircraft)
43 : {
44 248542 : gimbal.update(aircraft);
45 :
46 : // see if we should do a report
47 248542 : send_report();
48 248542 : }
49 :
50 : static struct gimbal_param {
51 : const char *name;
52 : float value;
53 : } gimbal_params[] = {
54 : {"GMB_OFF_ACC_X", 0},
55 : {"GMB_OFF_ACC_Y", 0},
56 : {"GMB_OFF_ACC_Z", 0},
57 : {"GMB_GN_ACC_X", 0},
58 : {"GMB_GN_ACC_Y", 0},
59 : {"GMB_GN_ACC_Z", 0},
60 : {"GMB_OFF_GYRO_X", 0},
61 : {"GMB_OFF_GYRO_Y", 0},
62 : {"GMB_OFF_GYRO_Z", 0},
63 : {"GMB_OFF_JNT_X", 0},
64 : {"GMB_OFF_JNT_Y", 0},
65 : {"GMB_OFF_JNT_Z", 0},
66 : {"GMB_K_RATE", 0},
67 : {"GMB_POS_HOLD", 0},
68 : {"GMB_MAX_TORQUE", 0},
69 : {"GMB_SND_TORQUE", 0},
70 : {"GMB_SYSID", 0},
71 : {"GMB_FLASH", 0},
72 : };
73 :
74 : /*
75 : find a parameter structure
76 : */
77 8 : struct gimbal_param *SoloGimbal::param_find(const char *name)
78 : {
79 120 : for (uint8_t i=0; i<ARRAY_SIZE(gimbal_params); i++) {
80 120 : if (strncmp(name, gimbal_params[i].name, 16) == 0) {
81 8 : return &gimbal_params[i];
82 : }
83 : }
84 0 : return nullptr;
85 : }
86 :
87 : /*
88 : send a parameter to flight board
89 : */
90 26 : void SoloGimbal::param_send(const struct gimbal_param *p)
91 : {
92 : mavlink_message_t msg;
93 26 : mavlink_param_value_t param_value{};
94 26 : strncpy_noterm(param_value.param_id, p->name, sizeof(param_value.param_id));
95 26 : param_value.param_value = p->value;
96 26 : param_value.param_count = 0;
97 26 : param_value.param_index = 0;
98 26 : param_value.param_type = MAV_PARAM_TYPE_REAL32;
99 :
100 26 : uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id,
101 26 : gimbal_component_id,
102 : &mavlink.status,
103 : &msg, ¶m_value);
104 :
105 26 : uint8_t msgbuf[len];
106 26 : len = mavlink_msg_to_send_buffer(msgbuf, &msg);
107 26 : if (len > 0) {
108 26 : mav_socket.send(msgbuf, len);
109 : }
110 26 : }
111 :
112 :
113 : /*
114 : send a report to the vehicle control code over MAVLink
115 : */
116 248542 : void SoloGimbal::send_report(void)
117 : {
118 248542 : uint32_t now = AP_HAL::millis();
119 248542 : if (now < 10000) {
120 : // don't send gimbal reports until 10s after startup. This
121 : // avoids a windows threading issue with non-blocking sockets
122 : // and the initial wait on SERIAL0
123 12008 : return;
124 : }
125 236537 : if (!mavlink.connected) {
126 1 : if (target_path != nullptr && mav_socket.connect_unix(target_path)) {
127 0 : ::printf("SoloGimbal connected to Unix domain socket %s\n", target_path);
128 0 : mavlink.connected = true;
129 1 : } else if (target_path == nullptr && mav_socket.connect(target_address, target_port)) {
130 1 : ::printf("SoloGimbal connected to %s:%u\n", target_address, (unsigned)target_port);
131 1 : mavlink.connected = true;
132 : }
133 : }
134 236537 : if (!mavlink.connected) {
135 0 : return;
136 : }
137 :
138 236537 : if (param_send_last_ms && now - param_send_last_ms > 100) {
139 18 : param_send(&gimbal_params[param_send_idx]);
140 18 : if (++param_send_idx == ARRAY_SIZE(gimbal_params)) {
141 1 : printf("Finished sending parameters\n");
142 1 : param_send_last_ms = 0;
143 : }
144 : }
145 :
146 : // check for incoming MAVLink messages
147 : uint8_t buf[100];
148 : ssize_t ret;
149 :
150 236749 : while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) {
151 5083 : for (uint8_t i=0; i<ret; i++) {
152 : mavlink_message_t msg;
153 : mavlink_status_t status;
154 9742 : if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
155 4871 : buf[i],
156 4871 : &msg, &status) == MAVLINK_FRAMING_OK) {
157 218 : switch (msg.msgid) {
158 201 : case MAVLINK_MSG_ID_HEARTBEAT: {
159 : mavlink_heartbeat_t pkt;
160 201 : mavlink_msg_heartbeat_decode(&msg, &pkt);
161 : debug("got HB type=%u autopilot=%u base_mode=0x%x\n", pkt.type, pkt.autopilot, pkt.base_mode);
162 201 : if (!seen_heartbeat) {
163 1 : seen_heartbeat = true;
164 1 : vehicle_component_id = msg.compid;
165 1 : vehicle_system_id = msg.sysid;
166 1 : ::printf("SoloGimbal using srcSystem %u\n", (unsigned)vehicle_system_id);
167 : }
168 201 : break;
169 : }
170 0 : case MAVLINK_MSG_ID_GIMBAL_CONTROL: {
171 : static uint32_t counter;
172 0 : if (counter++ % 100 == 0) {
173 0 : printf("GIMBAL_CONTROL %u\n", counter);
174 : }
175 : mavlink_gimbal_control_t pkt;
176 0 : mavlink_msg_gimbal_control_decode(&msg, &pkt);
177 0 : gimbal.set_demanded_rates(Vector3f(pkt.demanded_rate_x,
178 : pkt.demanded_rate_y,
179 : pkt.demanded_rate_z));
180 0 : seen_gimbal_control = true;
181 0 : break;
182 : }
183 8 : case MAVLINK_MSG_ID_PARAM_SET: {
184 : mavlink_param_set_t pkt;
185 8 : mavlink_msg_param_set_decode(&msg, &pkt);
186 8 : printf("SoloGimbal got PARAM_SET %.16s %f\n", pkt.param_id, pkt.param_value);
187 :
188 8 : struct gimbal_param *p = param_find(pkt.param_id);
189 8 : if (p) {
190 8 : p->value = pkt.param_value;
191 8 : param_send(p);
192 : }
193 :
194 8 : break;
195 : }
196 1 : case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
197 : mavlink_param_request_list_t pkt;
198 1 : mavlink_msg_param_request_list_decode(&msg, &pkt);
199 1 : if (pkt.target_system == 0 && pkt.target_component == MAV_COMP_ID_GIMBAL) {
200 : // start param send
201 1 : param_send_idx = 0;
202 1 : param_send_last_ms = AP_HAL::millis();
203 : }
204 1 : printf("SoloGimbal sending %u parameters\n", (unsigned)ARRAY_SIZE(gimbal_params));
205 1 : break;
206 : }
207 8 : default:
208 : debug("got unexpected msg %u\n", msg.msgid);
209 8 : break;
210 : }
211 : }
212 : }
213 : }
214 :
215 236537 : if (!seen_heartbeat) {
216 3 : return;
217 : }
218 : mavlink_message_t msg;
219 : uint16_t len;
220 :
221 236534 : if (now - last_heartbeat_ms >= 1000) {
222 : mavlink_heartbeat_t heartbeat;
223 198 : heartbeat.type = MAV_TYPE_GIMBAL;
224 198 : heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
225 198 : heartbeat.base_mode = 0;
226 198 : heartbeat.system_status = 0;
227 198 : heartbeat.mavlink_version = 0;
228 198 : heartbeat.custom_mode = 0;
229 :
230 198 : len = mavlink_msg_heartbeat_encode_status(vehicle_system_id,
231 198 : gimbal_component_id,
232 : &mavlink.status,
233 : &msg, &heartbeat);
234 :
235 198 : mav_socket.send(&msg.magic, len);
236 198 : last_heartbeat_ms = now;
237 : }
238 :
239 : /*
240 : send a GIMBAL_REPORT message
241 : */
242 236534 : uint32_t now_us = AP_HAL::micros();
243 236534 : if (now_us - last_report_us > reporting_period_ms*1000UL) {
244 18195 : last_report_us = now_us;
245 :
246 : uint32_t delta_time_us;
247 18195 : Vector3f delta_angle;
248 18195 : Vector3f delta_velocity;
249 18195 : gimbal.get_deltas(delta_angle, delta_velocity, delta_time_us);
250 :
251 18195 : Vector3f joint_angles;
252 18195 : gimbal.get_joint_angles(joint_angles);
253 :
254 : mavlink_gimbal_report_t gimbal_report;
255 18195 : gimbal_report.target_system = vehicle_system_id;
256 18195 : gimbal_report.target_component = vehicle_component_id;
257 18195 : gimbal_report.delta_time = delta_time_us * 1e-6;
258 18195 : gimbal_report.delta_angle_x = delta_angle.x;
259 18195 : gimbal_report.delta_angle_y = delta_angle.y;
260 18195 : gimbal_report.delta_angle_z = delta_angle.z;
261 18195 : gimbal_report.delta_velocity_x = delta_velocity.x;
262 18195 : gimbal_report.delta_velocity_y = delta_velocity.y;
263 18195 : gimbal_report.delta_velocity_z = delta_velocity.z;
264 18195 : gimbal_report.joint_roll = joint_angles.x;
265 18195 : gimbal_report.joint_el = joint_angles.y;
266 18195 : gimbal_report.joint_az = joint_angles.z;
267 :
268 18195 : len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id,
269 18195 : gimbal_component_id,
270 : &mavlink.status,
271 : &msg, &gimbal_report);
272 :
273 18195 : uint8_t msgbuf[len];
274 18195 : len = mavlink_msg_to_send_buffer(msgbuf, &msg);
275 18195 : if (len > 0) {
276 18195 : mav_socket.send(msgbuf, len);
277 : }
278 :
279 18195 : delta_velocity.zero();
280 18195 : delta_angle.zero();
281 18195 : }
282 : }
283 :
284 : } // namespace SITL
285 :
286 : #endif // AP_SIM_SOLOGIMBAL_ENABLED
|
