Skip to content

Commit 5916480

Browse files
xznhj8129xznhj8129
committed
Lightened fc_mavlink + command/guided files + small refactor
1 parent 9df78a8 commit 5916480

File tree

10 files changed

+610
-505
lines changed

10 files changed

+610
-505
lines changed

‎src/main/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -395,6 +395,10 @@ main_sources(COMMON_SRC
395395

396396
io/osd/custom_elements.c
397397

398+
mavlink/mavlink_command.c
399+
mavlink/mavlink_command.h
400+
mavlink/mavlink_guided.c
401+
mavlink/mavlink_guided.h
398402
mavlink/mavlink_internal.h
399403
mavlink/mavlink_mission.c
400404
mavlink/mavlink_mission.h

‎src/main/fc/fc_mavlink.c‎

Lines changed: 17 additions & 368 deletions
Large diffs are not rendered by default.

‎src/main/mavlink/mavlink_command.c‎

Lines changed: 245 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,245 @@
1+
#include "mavlink/mavlink_internal.h"
2+
3+
#include "mavlink/mavlink_command.h"
4+
#include "mavlink/mavlink_guided.h"
5+
#include "mavlink/mavlink_runtime.h"
6+
#include "mavlink/mavlink_streams.h"
7+
8+
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK)
9+
10+
static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent)
11+
{
12+
if (targetSystem != 0 && targetSystem != mavSystemId) {
13+
return false;
14+
}
15+
16+
if (targetComponent != 0 && targetComponent != mavComponentId) {
17+
return false;
18+
}
19+
20+
return true;
21+
}
22+
23+
static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t ackTargetSystem, uint8_t ackTargetComponent)
24+
{
25+
mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
26+
command,
27+
result,
28+
0,
29+
0,
30+
ackTargetSystem,
31+
ackTargetComponent);
32+
mavlinkSendMessage();
33+
}
34+
35+
static bool handleIncoming_COMMAND(
36+
uint8_t targetSystem,
37+
uint8_t targetComponent,
38+
uint8_t ackTargetSystem,
39+
uint8_t ackTargetComponent,
40+
uint16_t command,
41+
uint8_t frame,
42+
float param1,
43+
float param2,
44+
float param3,
45+
float param4,
46+
float latitudeDeg,
47+
float longitudeDeg,
48+
float altitudeMeters)
49+
{
50+
if (!mavlinkIsLocalTarget(targetSystem, targetComponent)) {
51+
return false;
52+
}
53+
UNUSED(param3);
54+
55+
switch (command) {
56+
case MAV_CMD_DO_REPOSITION:
57+
if (!mavlinkFrameIsSupported(frame,
58+
MAV_FRAME_SUPPORTED_GLOBAL |
59+
MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
60+
MAV_FRAME_SUPPORTED_GLOBAL_INT |
61+
MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
62+
mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
63+
return true;
64+
}
65+
66+
if (isnan(latitudeDeg) || isnan(longitudeDeg)) {
67+
mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent);
68+
return true;
69+
}
70+
71+
if (isGCSValid()) {
72+
navWaypoint_t wp = {0};
73+
wp.action = NAV_WP_ACTION_WAYPOINT;
74+
wp.lat = (int32_t)(latitudeDeg * 1e7f);
75+
wp.lon = (int32_t)(longitudeDeg * 1e7f);
76+
wp.alt = (int32_t)(altitudeMeters * 100.0f);
77+
if (!isnan(param4) && param4 >= 0.0f && param4 < 360.0f) {
78+
wp.p1 = (int16_t)param4;
79+
} else {
80+
wp.p1 = 0;
81+
}
82+
wp.p2 = 0;
83+
wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
84+
wp.flag = 0;
85+
86+
setWaypoint(255, &wp);
87+
88+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
89+
} else {
90+
mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent);
91+
}
92+
return true;
93+
case MAV_CMD_DO_CHANGE_ALTITUDE:
94+
{
95+
const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1);
96+
mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent);
97+
return true;
98+
}
99+
case MAV_CMD_CONDITION_YAW:
100+
{
101+
if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) {
102+
mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent);
103+
return true;
104+
}
105+
106+
int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f));
107+
108+
if (param4 != 0.0f) {
109+
const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw;
110+
const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f);
111+
112+
if (param3 < 0.0f) {
113+
targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd);
114+
} else {
115+
targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd);
116+
}
117+
}
118+
119+
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd));
120+
posControl.desiredState.yaw = targetHeadingCd;
121+
posControl.cruise.course = targetHeadingCd;
122+
posControl.cruise.previousCourse = targetHeadingCd;
123+
124+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
125+
return true;
126+
}
127+
case MAV_CMD_SET_MESSAGE_INTERVAL:
128+
{
129+
mavlinkPeriodicMessage_e periodicMessage;
130+
MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
131+
132+
if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) {
133+
if (param2 == 0.0f) {
134+
mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0);
135+
result = MAV_RESULT_ACCEPTED;
136+
} else if (param2 < 0.0f) {
137+
mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1);
138+
result = MAV_RESULT_ACCEPTED;
139+
} else {
140+
uint32_t intervalUs = (uint32_t)param2;
141+
if (intervalUs > 0) {
142+
const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE;
143+
if (intervalUs < minIntervalUs) {
144+
intervalUs = minIntervalUs;
145+
}
146+
147+
mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs);
148+
result = MAV_RESULT_ACCEPTED;
149+
}
150+
}
151+
}
152+
153+
mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent);
154+
return true;
155+
}
156+
case MAV_CMD_GET_MESSAGE_INTERVAL:
157+
{
158+
mavlinkPeriodicMessage_e periodicMessage;
159+
if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) {
160+
mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
161+
return true;
162+
}
163+
164+
mavlink_msg_message_interval_pack(
165+
mavSystemId,
166+
mavComponentId,
167+
&mavSendMsg,
168+
(uint16_t)param1,
169+
mavlinkMessageIntervalUs(periodicMessage));
170+
mavlinkSendMessage();
171+
172+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
173+
return true;
174+
}
175+
case MAV_CMD_CONTROL_HIGH_LATENCY:
176+
if (param1 == 0.0f || param1 == 1.0f) {
177+
if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) {
178+
mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
179+
return true;
180+
}
181+
182+
mavActivePort->highLatencyEnabled = param1 > 0.5f;
183+
if (mavActivePort->highLatencyEnabled) {
184+
mavActivePort->lastHighLatencyMessageUs = 0;
185+
}
186+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
187+
} else {
188+
mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent);
189+
}
190+
return true;
191+
case MAV_CMD_REQUEST_PROTOCOL_VERSION:
192+
if (mavlinkGetProtocolVersion() == 1) {
193+
mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
194+
} else {
195+
mavlinkSendProtocolVersion();
196+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
197+
}
198+
return true;
199+
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
200+
if (mavlinkGetProtocolVersion() == 1) {
201+
mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
202+
} else {
203+
mavlinkSendAutopilotVersion();
204+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
205+
}
206+
return true;
207+
case MAV_CMD_REQUEST_MESSAGE:
208+
{
209+
const bool sent = mavlinkSendRequestedMessage((uint16_t)param1);
210+
mavlinkSendCommandAck(command, sent ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
211+
return true;
212+
}
213+
#ifdef USE_GPS
214+
case MAV_CMD_GET_HOME_POSITION:
215+
if (mavlinkSendRequestedMessage(MAVLINK_MSG_ID_HOME_POSITION)) {
216+
mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
217+
} else {
218+
mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent);
219+
}
220+
return true;
221+
#endif
222+
default:
223+
mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
224+
return true;
225+
}
226+
}
227+
228+
bool mavlinkHandleIncomingCommandInt(void)
229+
{
230+
mavlink_command_int_t msg;
231+
mavlink_msg_command_int_decode(&mavlinkContext.recvMsg, &msg);
232+
233+
return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, msg.frame, msg.param1, msg.param2, msg.param3, msg.param4, (float)msg.x / 1e7f, (float)msg.y / 1e7f, msg.z);
234+
}
235+
236+
bool mavlinkHandleIncomingCommandLong(void)
237+
{
238+
mavlink_command_long_t msg;
239+
mavlink_msg_command_long_decode(&mavlinkContext.recvMsg, &msg);
240+
241+
// COMMAND_LONG has no frame field; location commands are WGS84 global by definition.
242+
return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7);
243+
}
244+
245+
#endif

‎src/main/mavlink/mavlink_command.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
#pragma once
2+
3+
#include <stdbool.h>
4+
#include <stdint.h>
5+
6+
bool mavlinkHandleIncomingCommandLong(void);
7+
bool mavlinkHandleIncomingCommandInt(void);

0 commit comments

Comments
 (0)