1
1
package mavlink
2
2
3
3
import (
4
- _ "embed"
5
4
"errors"
6
5
"fmt"
7
- "log"
8
6
"reflect"
9
7
"regexp"
10
8
"strconv"
@@ -35,7 +33,7 @@ func Contains(slice []string, str string) bool {
35
33
func MavlinkEventFrameToMetric (frm * gomavlib.EventFrame ) MetricFrameData {
36
34
out := MetricFrameData {}
37
35
out .tags = map [string ]string {}
38
- out .fields = make (map [string ]interface {} )
36
+ out .fields = make (map [string ]any )
39
37
out .tags ["sys_id" ] = fmt .Sprintf ("%d" , frm .SystemID ())
40
38
41
39
m := frm .Message ()
@@ -53,88 +51,86 @@ func MavlinkEventFrameToMetric(frm *gomavlib.EventFrame) MetricFrameData {
53
51
}
54
52
55
53
out .name = ConvertToSnakeCase (t .Name ())
56
- if strings .HasPrefix (out .name , "message_" ) {
57
- out .name = strings .TrimPrefix (out .name , "message_" )
58
- }
59
-
54
+ out .name = strings .TrimPrefix (out .name , "message_" )
60
55
return out
61
56
}
62
57
63
- // Parse the FcuUrl config to setup a mavlib endpoint config
58
+ // Parse the FcuURL config to setup a mavlib endpoint config
64
59
func ParseMavlinkEndpointConfig (s * Mavlink ) ([]gomavlib.EndpointConf , error ) {
65
- if strings .HasPrefix (s .FcuUrl , "serial://" ) {
66
- tmpStr := strings .TrimPrefix (s .FcuUrl , "serial://" )
60
+ if strings .HasPrefix (s .FcuURL , "serial://" ) {
61
+ tmpStr := strings .TrimPrefix (s .FcuURL , "serial://" )
67
62
tmpStrParts := strings .Split (tmpStr , ":" )
68
63
deviceName := tmpStrParts [0 ]
69
64
baudRate := 57600
70
65
if len (tmpStrParts ) == 2 {
71
66
newBaudRate , err := strconv .Atoi (tmpStrParts [1 ])
72
67
if err != nil {
73
- return nil , errors .New ("Mavlink setup error: serial baud rate not valid! " )
68
+ return nil , errors .New ("mavlink setup error: serial baud rate not valid" )
74
69
}
75
70
baudRate = newBaudRate
76
71
}
77
72
78
- log .Printf ("Mavlink serial client: device %s, baud rate %d" , deviceName , baudRate )
79
73
return []gomavlib.EndpointConf {
80
74
gomavlib.EndpointSerial {
81
75
Device : deviceName ,
82
76
Baud : baudRate ,
83
77
},
84
78
}, nil
85
- } else if strings .HasPrefix (s .FcuUrl , "tcp://" ) {
79
+ } else if strings .HasPrefix (s .FcuURL , "tcp://" ) {
86
80
// TCP client
87
- tmpStr := strings .TrimPrefix (s .FcuUrl , "tcp://" )
81
+ tmpStr := strings .TrimPrefix (s .FcuURL , "tcp://" )
88
82
tmpStrParts := strings .Split (tmpStr , ":" )
89
83
if len (tmpStrParts ) != 2 {
90
- return nil , errors .New ("Mavlink setup error: TCP requires a port! " )
84
+ return nil , errors .New ("mavlink setup error: TCP requires a port" )
91
85
}
92
86
93
87
hostname := tmpStrParts [0 ]
94
- port := 14550
95
88
port , err := strconv .Atoi (tmpStrParts [1 ])
96
89
if err != nil {
97
- return nil , errors .New ("Mavlink setup error: TCP port is invalid! " )
90
+ return nil , errors .New ("mavlink setup error: TCP port is invalid" )
98
91
}
99
92
100
93
if len (hostname ) > 0 {
101
- log .Printf ("Mavlink TCP client: hostname %s, port %d" , hostname , port )
102
94
return []gomavlib.EndpointConf {
103
- gomavlib.EndpointTCPClient {fmt .Sprintf ("%s:%d" , hostname , port )},
95
+ gomavlib.EndpointTCPClient {
96
+ Address : fmt .Sprintf ("%s:%d" , hostname , port ),
97
+ },
104
98
}, nil
105
99
} else {
106
- log .Printf ("Mavlink TCP server: port %d" , port )
107
100
return []gomavlib.EndpointConf {
108
- gomavlib.EndpointTCPServer {fmt .Sprintf (":%d" , port )},
101
+ gomavlib.EndpointTCPServer {
102
+ Address : fmt .Sprintf (":%d" , port ),
103
+ },
109
104
}, nil
110
105
}
111
- } else if strings .HasPrefix (s .FcuUrl , "udp://" ) {
106
+ } else if strings .HasPrefix (s .FcuURL , "udp://" ) {
112
107
// UDP client or server
113
- tmpStr := strings .TrimPrefix (s .FcuUrl , "udp://" )
108
+ tmpStr := strings .TrimPrefix (s .FcuURL , "udp://" )
114
109
tmpStrParts := strings .Split (tmpStr , ":" )
115
110
if len (tmpStrParts ) != 2 {
116
- return nil , errors .New ("Mavlink setup error: UDP requires a port! " )
111
+ return nil , errors .New ("mavlink setup error: UDP requires a port" )
117
112
}
118
113
119
114
hostname := tmpStrParts [0 ]
120
- port := 14550
121
115
port , err := strconv .Atoi (tmpStrParts [1 ])
122
116
if err != nil {
123
- return nil , errors .New ("Mavlink setup error: UDP port is invalid! " )
117
+ return nil , errors .New ("mavlink setup error: UDP port is invalid" )
124
118
}
125
119
126
120
if len (hostname ) > 0 {
127
- log .Printf ("Mavlink UDP client: hostname %s, port %d" , hostname , port )
128
121
return []gomavlib.EndpointConf {
129
- gomavlib.EndpointUDPClient {fmt .Sprintf ("%s:%d" , hostname , port )},
122
+ gomavlib.EndpointUDPClient {
123
+ Address : fmt .Sprintf ("%s:%d" , hostname , port ),
124
+ },
130
125
}, nil
131
126
} else {
132
- log .Printf ("Mavlink UDP server: port %d" , port )
133
127
return []gomavlib.EndpointConf {
134
- gomavlib.EndpointUDPServer {fmt .Sprintf (":%d" , port )},
128
+ gomavlib.EndpointUDPServer {
129
+ Address : fmt .Sprintf (":%d" , port ),
130
+ },
135
131
}, nil
136
132
}
137
133
} else {
138
- return nil , errors .New ("Mavlink setup error: invalid fcu_url! " )
134
+ return nil , errors .New ("mavlink setup error: invalid fcu_url" )
139
135
}
140
136
}
0 commit comments