Skip to content

Commit b1dd76a

Browse files
committed
feat: add new SRx_OPTIONS bitmask parameter with a mavlink2-signing bit
1 parent fcbd854 commit b1dd76a

File tree

3 files changed

+33
-1
lines changed

3 files changed

+33
-1
lines changed

libraries/GCS_MAVLink/GCS.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,12 @@ class GCS_MAVLINK_InProgress
163163
static uint32_t last_check_ms;
164164
};
165165

166+
enum class SROption : int16_t
167+
{
168+
None = 0,
169+
MAVLINK2_SIGNING = (1U << 0),
170+
};
171+
166172
///
167173
/// @class GCS_MAVLINK
168174
/// @brief MAVLink transport control class
@@ -512,6 +518,11 @@ class GCS_MAVLINK
512518
virtual bool persist_streamrates() const { return false; }
513519
void handle_request_data_stream(const mavlink_message_t &msg);
514520

521+
AP_Int16 options;
522+
bool option_enabled(SROption option) const {
523+
return options & static_cast<int16_t>(option);
524+
}
525+
515526
virtual void handle_command_ack(const mavlink_message_t &msg);
516527
void handle_set_mode(const mavlink_message_t &msg);
517528
void handle_command_int(const mavlink_message_t &msg);

libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,9 @@ const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
2222

2323
// tradition has different vehicles use different default stream rates. This may not be the case forever, but for now we maintain this behaviour:
2424

25+
// Unified signing options across all vehicles
26+
#define STANDARD_DEFAULT_STREAM_RATE_OPTIONS (SROption::MAVLINK2_SIGNING)
27+
2528
#ifndef AP_MAV_DEFAULT_STREAM_RATE_RAW_SENS
2629
#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
2730
#define AP_MAV_DEFAULT_STREAM_RATE_RAW_SENS 2
@@ -34,6 +37,7 @@ const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
3437
#define AP_MAV_DEFAULT_STREAM_RATE_EXTRA3 3
3538
#define AP_MAV_DEFAULT_STREAM_RATE_PARAMS 0
3639
#define AP_MAV_DEFAULT_STREAM_RATE_ADSB 0
40+
#define AP_MAV_DEFAULT_STREAM_RATE_OPTIONS STANDARD_DEFAULT_STREAM_RATE_OPTIONS
3741
#elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker) || APM_BUILD_TYPE(APM_BUILD_Rover)
3842
#define AP_MAV_DEFAULT_STREAM_RATE_RAW_SENS 1
3943
#define AP_MAV_DEFAULT_STREAM_RATE_EXT_STAT 1
@@ -45,6 +49,7 @@ const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
4549
#define AP_MAV_DEFAULT_STREAM_RATE_EXTRA3 1
4650
#define AP_MAV_DEFAULT_STREAM_RATE_PARAMS 10
4751
#define AP_MAV_DEFAULT_STREAM_RATE_ADSB 0
52+
#define AP_MAV_DEFAULT_STREAM_RATE_OPTIONS STANDARD_DEFAULT_STREAM_RATE_OPTIONS
4853
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
4954
#define AP_MAV_DEFAULT_STREAM_RATE_RAW_SENS 1
5055
#define AP_MAV_DEFAULT_STREAM_RATE_EXT_STAT 1
@@ -56,6 +61,7 @@ const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
5661
#define AP_MAV_DEFAULT_STREAM_RATE_EXTRA3 1
5762
#define AP_MAV_DEFAULT_STREAM_RATE_PARAMS 10
5863
#define AP_MAV_DEFAULT_STREAM_RATE_ADSB 5
64+
#define AP_MAV_DEFAULT_STREAM_RATE_OPTIONS STANDARD_DEFAULT_STREAM_RATE_OPTIONS
5965
#elif APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Blimp) || APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_AP_Periph)
6066
#define AP_MAV_DEFAULT_STREAM_RATE_RAW_SENS 0
6167
#define AP_MAV_DEFAULT_STREAM_RATE_EXT_STAT 0
@@ -67,6 +73,7 @@ const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
6773
#define AP_MAV_DEFAULT_STREAM_RATE_EXTRA3 0
6874
#define AP_MAV_DEFAULT_STREAM_RATE_PARAMS 0
6975
#define AP_MAV_DEFAULT_STREAM_RATE_ADSB 0
76+
#define AP_MAV_DEFAULT_STREAM_RATE_OPTIONS STANDARD_DEFAULT_STREAM_RATE_OPTIONS
7077
#elif APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
7178
#define AP_MAV_DEFAULT_STREAM_RATE_RAW_SENS 0
7279
#define AP_MAV_DEFAULT_STREAM_RATE_EXT_STAT 0
@@ -78,6 +85,7 @@ const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
7885
#define AP_MAV_DEFAULT_STREAM_RATE_EXTRA3 0
7986
#define AP_MAV_DEFAULT_STREAM_RATE_PARAMS 0
8087
#define AP_MAV_DEFAULT_STREAM_RATE_ADSB 0
88+
#define AP_MAV_DEFAULT_STREAM_RATE_OPTIONS STANDARD_DEFAULT_STREAM_RATE_OPTIONS
8189
#else
8290
#error Need to set streamrates
8391
#endif // APM_BUILD_TYPE
@@ -202,6 +210,18 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
202210
// @RebootRequired: True
203211
// @User: Advanced
204212
AP_GROUPINFO("_ADSB", 10, GCS_MAVLINK, streamRates[GCS_MAVLINK::STREAM_ADSB], DRATE(GCS_MAVLINK::STREAM_ADSB)),
213+
214+
// ------------
215+
// IMPORTANT: Add new stream rates *before* the _OPTIONS parameter.
216+
// ------------
217+
218+
// @Param: _OPTIONS
219+
// @DisplayName: Bitmask for configuring this telemetry channel
220+
// @Description: Bitmask for configuring this telemetry channel. For having effect on all channels, set the relevant mask in all SRx_OPTIONS parameters. Keep in mind that part of the flags may require a reboot to take action.
221+
// @RebootRequired: True
222+
// @User: Standard
223+
// @Bitmask: 0:Enable MavLink2 signing (Reboot required)
224+
AP_GROUPINFO("_OPTIONS", 11, GCS_MAVLINK, options, float(AP_MAV_DEFAULT_STREAM_RATE_OPTIONS)),
205225
AP_GROUPEND
206226
};
207227
#undef DRATE

libraries/GCS_MAVLink/GCS_Signing.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -133,7 +133,8 @@ static bool accept_unsigned_callback(const mavlink_status_t *status, uint32_t ms
133133
void GCS_MAVLINK::load_signing_key(void)
134134
{
135135
struct SigningKey key;
136-
if (!signing_key_load(key)) {
136+
if (!option_enabled(SROption::MAVLINK2_SIGNING) || !signing_key_load(key))
137+
{
137138
return;
138139
}
139140
memcpy(signing.secret_key, key.secret_key, 32);

0 commit comments

Comments
 (0)