mirror of https://github.com/ArduPilot/ardupilot
56 lines
1.6 KiB
Mathematica
56 lines
1.6 KiB
Mathematica
|
clc
|
||
|
|
clear
|
||
|
|
|
||
|
|
% This is a example of using MATLAB to test a sensor drive using TCP
|
||
|
|
% we send a NMEA messages at 1hz, note that this assumes that SITL is
|
||
|
|
% running at real time.
|
||
|
|
|
||
|
|
% NMEA is a basic one way protocol but more complex protocols can be
|
||
|
|
% implemented in the same way
|
||
|
|
|
||
|
|
% Use the same TCP/UDP libbary that is used for MALTAB SITL
|
||
|
|
addpath(genpath('../../SITL/examples/JSON/MATLAB/tcp_udp_ip_2.0.6'))
|
||
|
|
|
||
|
|
% if this dosn't work try the MALTAB SITL example first
|
||
|
|
pnet('closeall')
|
||
|
|
|
||
|
|
% Init the TCP port, 5763 is serial 2
|
||
|
|
u = pnet('tcpconnect','127.0.0.1',5763);
|
||
|
|
|
||
|
|
flipflop = true;
|
||
|
|
while(true)
|
||
|
|
|
||
|
|
if flipflop
|
||
|
|
% send MTW temp message
|
||
|
|
water_temp = 10 + randn();
|
||
|
|
NMEA_string = sprintf('$YXMTW,%0.1f,C',water_temp);
|
||
|
|
else
|
||
|
|
% send VHW speed message
|
||
|
|
water_speed_knots = 5 + randn()*2;
|
||
|
|
water_speed_kph = water_speed_knots * 1.852;
|
||
|
|
NMEA_string = sprintf('$VWVHW,,T,,M,%0.1f,N,%0.1f,F',water_speed_knots,water_speed_kph);
|
||
|
|
end
|
||
|
|
flipflop = ~flipflop;
|
||
|
|
|
||
|
|
% Calculate the correct checksum
|
||
|
|
NMEA_string = add_checksum(NMEA_string);
|
||
|
|
|
||
|
|
% send to ap
|
||
|
|
pnet(u,'printf',sprintf('%s\r\n',NMEA_string));
|
||
|
|
pnet(u,'writepacket');
|
||
|
|
|
||
|
|
% also print to MATLAB console
|
||
|
|
fprintf("%s\n",NMEA_string);
|
||
|
|
|
||
|
|
% 1hz (ish)
|
||
|
|
pause(1);
|
||
|
|
end
|
||
|
|
|
||
|
|
function NMEA_string_out = add_checksum(NMEA_string_in)
|
||
|
|
checksum = uint8(0);
|
||
|
|
for i = 2:numel(NMEA_string_in)
|
||
|
|
checksum = bitxor(checksum,uint8(NMEA_string_in(i)),'uint8');
|
||
|
|
end
|
||
|
|
NMEA_string_out = sprintf('%s*%s',NMEA_string_in,dec2hex(checksum));
|
||
|
|
end
|