%% Use a MAVLink Parameter Protocol for Tuning UAV Parameters in MATLAB
% This example shows how to use a MAVLink parameter protocol in MATLAB and communicate with external ground control stations. A sample parameter protocol is provided for sending parameter updates from a simulated unmanned aerial vehicle (UAV) to a ground control station using MAVLink communication protocols. You setup the communication between the two MAVLink components, the UAV and the ground control station. Then, you send and receive parameter updates to tune parameter values for the UAV. Finally, if you use QGroundControl© as a ground control station, you can get these parameter updates from QGroundControl and see them reflected in the program window.
%% Parameter Protocol
% MAVLink clients exchange information within the network using commonly defined data structures as messages. MAVLink parameter protocol is used to exchange configuration settings between UAV and ground control station (GCS). Parameter protocol follows a client-server pattern. For example, GCS initiates a request in the form of messages and the UAV responds with data.
%% Setup common dialect
% MAVLink messages are defined in an XML file. Standard messages that are common to all systems are defined in the "common.xml" file. Other vendor-specific messages are stored in separate XML files. For this example, use the "common.xml" file to setup a common dialect between the MAVLink clients.

dialect = mavlinkdialect("common.xml");

% This dialect is used to create mavlinkio objects which can understand messages within the dialect.
%
%% Setup UAV Connection
% Create a mavlinkio object to represent a simulated UAV. Specify the SystemID, ComponentID, AutoPilotType, and ComponentType parameters as name-value pairs. For this example, we use a generic autopilot type, 'MAV_AUTOPILOT_GENERIC', with a quadrotor component type, 'MAV_TYPE_QUADROTOR'.

uavNode = mavlinkio(dialect,'SystemID',1,'ComponentID',1, ...
    'AutopilotType',"MAV_AUTOPILOT_GENERIC",'ComponentType',"MAV_TYPE_QUADROTOR");

% The simulated UAV is listening on a UDP port for incoming messages. Connect to this UDP port using the uavNode object.

uavPort = 14750;
connect(uavNode,"UDP",'LocalPort',uavPort);

%
%% Setup GCS Connection
% Create a simulated ground control station (GCS) that listens on a different UDP port.

gcsNode = mavlinkio(dialect);
gcsPort = 14560;
connect(gcsNode,"UDP", 'LocalPort', gcsPort);

%
%% Setup Client and Subscriber
% Setup a client interface for the simulated UAV to communicate with the ground control station. Get the LocalClient information as a structure and specify the system and component ID info to the mavlinkclient object.

clientStruct = uavNode.LocalClient;
uavClient = mavlinkclient(gcsNode,clientStruct.SystemID,clientStruct.ComponentID);

% Create a mavlinksub object to receive messages and process those messages using a callback. This subscriber receives messages on the 'PARAM_VALUE' topic and specifically looks for messages matching the system and component ID of uavClient. A callback function is specified to display the payload of each new message received.

paramValueSub = mavlinksub(gcsNode,uavClient,'PARAM_VALUE','BufferSize',10,...
                            'NewMessageFcn', @(~,msg)disp(msg.Payload));

%
%% Parameter Operations
% Now that you have setup the connections between the UAV and ground control station. You can now query and update the simulated UAV configuration using operations defined in parameter protocol, exampleHelperMAVParamProtocol. There are 4 GCS operations that describe the workflow of parameter protocol.  Each message type listed has a brief description what the message executes based on the specified parameter protocol.
% PARAM_REQUET_LIST: Requests all parameters from the recipients. All values are broadcasted using PARAM_VALUE messages.
% PARAM_REQUEST_READ: Requests a single parameter. The specified parameter value is broadcasted using a PARAM_VALUE message.
% PARAM_SET: Commands to set the value of the specific parameter. After setting up the value, the current value is broadcasted using a PARAM_VALUE message.
% PARAM_VALUE: Broadcasts the current value of a parameter in response to the above requests (PARAM_REQUEST_LIST, PARAM_REQUEST_READ or PARAM_SET).

paramProtocol = exampleHelperMAVParamProtocol(uavNode);

% This parameter protocol has three parameter values: 'MAX_ROLL_RATE', 'MAX_PITCH_RATE', and 'MAX_YAW_RATE'.  These values represent the maximum rate for roll, pitch, and yaw for the UAV in degrees per second. In a real UAV systems, these rates can be tuned to adjust performance for more or less acrobatic control.
%% Read All Parameters
% To read all parameters from a UAV system, send a "PARAM_REQUEST_LIST" message from gcsNode to uavNode. The sequence of operations are as follows:
% GCS node sends a message whose topic is "PARAM_REQUEST_LIST" to the UAV node specifying the target system and component using uavClient as defined above.
% UAV node sends out all parameters individually in the form of "PARAM_VALUE" messages, since we have a subscriber on the GCS node which is subscribed to the topic 'PARAM_VALUE', message payload is being displayed right away.

msg = createmsg(dialect,"PARAM_REQUEST_LIST");

% Assign values for the system and component ID into the message, use (:)= indexing to make sure the assignment doesn't change the struct field data type.

msg.Payload.target_system(:) = uavNode.LocalClient.SystemID;
msg.Payload.target_component(:) = uavNode.LocalClient.ComponentID;

% Send the parameter request to the UAV, which is listening on a port at local host IP address '127.0.0.1'. Pause to allow the message to be processed. The parameter list is displayed in the command window.

sendudpmsg(gcsNode,msg,"127.0.0.1",uavPort)
pause(1);

%     param_value: 90
%     param_count: 3
%     param_index: 0
%        param_id: 'MAX_ROLL_RATE   '
%      param_type: 9
%
%     param_value: 90
%     param_count: 3
%     param_index: 1
%        param_id: 'MAX_YAW_RATE    '
%      param_type: 9
%
%     param_value: 90
%     param_count: 3
%     param_index: 2
%        param_id: 'MAX_PITCH_RATE  '
%      param_type: 9
%
%% Read Single Parameter
% Read a single parameter by sending a "PARAM_REQUEST_READ" message from the GCS node to the UAV node.Send a message on the "PARAM_REQUEST_READ" topic to the UAVnode. Specify the parameter index of 0, which refers to the 'MAX_ROLL_RATE' parameter. This index value queries the first parameter value.
% The UAV sends the updated parameter as a "PARAM_VALUE" message back to the GCS node. Because we setup a subscriber to the "PARAM_VALUE" on the GCS node, the message payload is displayed to the command window.

msg = createmsg(gcsNode.Dialect,"PARAM_REQUEST_READ");
msg.Payload.param_index(:) = 0;
msg.Payload.target_system(:) = uavNode.LocalClient.SystemID;
msg.Payload.target_component(:) = uavNode.LocalClient.ComponentID;

sendudpmsg(gcsNode,msg,"127.0.0.1",uavPort);
pause(1);

%     param_value: 90
%     param_count: 3
%     param_index: 0
%        param_id: 'MAX_ROLL_RATE   '
%      param_type: 9
%
%% Write Parameters
% To write a parameter, send a "PARAM_SET" message from GCS node to UAV node. Specify the ID, type, and value of the message and send using the gcsNode object. The UAV sends the updated parameter value back and the GCS subscriber displays the message payload. This message updates the maximum yaw rate of the UAV by reducing it to 45 degrees per second.

msg = createmsg(gcsNode.Dialect,"PARAM_SET");
msg.Payload.param_id(1:12) = "MAX_YAW_RATE";
msg.Payload.param_type(:) = 9;
msg.Payload.param_value(:) = 45;
msg.Payload.target_system(:) = uavNode.LocalClient.SystemID;
msg.Payload.target_component(:) = uavNode.LocalClient.ComponentID;

sendudpmsg(gcsNode,msg,"127.0.0.1", uavPort);
pause(1);

%     param_value: 45
%     param_count: 3
%     param_index: 2
%        param_id: 'MAX_YAW_RATE    '
%      param_type: 9
%
%% Working with QGroundControl
% QGroundControl© is an app that is used to perform flight control and mission planning for any MAVLink-enabled UAV. You can use QGroundControl as a GCS to demonstrate how to access parameters of our simulated UAV:
% Download and launch QGroundControl. Define qgcPort number as 14550, which is the default UDP port for the QGroundControl app.
% Create a heartbeat message.
% Send heartbeat message from UAV node to QGroundControl using the MATLAB timer object. By default, the timer object executes the TimerFcn every 1 second. The TimerFcn is a sendudpmsg call that sends the heartbeat message.
% Once QGroundControl receives the heartbeat from the simulated UAV, QGroundControl creates a Parameter panel widget for the user to read and update UAV parameters

qgcPort = 14550;
heartbeat = createmsg(dialect,"HEARTBEAT");
heartbeat.Payload.type(:) = enum2num(dialect,'MAV_TYPE',uavNode.LocalClient.ComponentType);
heartbeat.Payload.autopilot(:) = enum2num(dialect,'MAV_AUTOPILOT',uavNode.LocalClient.AutopilotType);
heartbeat.Payload.system_status(:) = enum2num(dialect,'MAV_STATE',"MAV_STATE_STANDBY");

heartbeatTimer = timer;
heartbeatTimer.ExecutionMode = 'fixedRate';
heartbeatTimer.TimerFcn = @(~,~)sendudpmsg(uavNode,heartbeat,'127.0.0.1',qgcPort);
start(heartbeatTimer);

% While the timer runs, QGroundControl shows it has received the heartbeast message and is connected to a UAV. In the Vehicle Setup tab, click Other > Misc to see the parameter values set are reflected in the app.
%
% Note: Because we use a generic autopilot type, "MAV_AUTOPILOT_GENERIC", QGroundControl does not recognize the connection as a known autopilot type. This does not affect the connection and the parameter values should still update as shown.
%
%% Close MAVLink connections
% After experimenting with the QGroundControl parameter widget, stop the heartbeatTimer to stop sending any more heartbeat messages. Delete the heartbeatTimer and the paramProtocol objects. Finally, disconnect the UAV and GCS nodes to clean up the communication between systems.

stop(heartbeatTimer);
delete(heartbeatTimer);
delete(paramProtocol);

disconnect(uavNode);
disconnect(gcsNode);

% Copyright 2018 The MathWorks, Inc.
%