function modeOut = ArduCopterMode(flag) % Return the flight mode number used in ArduCopter % ref: https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/defines.h#L34 % % Inputs : flag (0,1) (optional) sets numeric mode as the key (default) or % text description (flag = 1) % % Outputs: modeOut : container.Map object valueSet = {'STABILIZE','ACRO','ALT_HOLD','AUTO','GUIDED','LOITER','RTL','CIRCLE',... 'LAND','DRIFT','SPORT','FLIP','AUTOTUNE','POSHOLD','BRAKE','THROW','AVOID_ADSB',... 'GUIDED_NOGPS','SMART_RTL','FLOWHOLD','FOLLOW','ZIGZAG','SYSTEMID', 'HELI_AUTOROTATE', 'AUTORTL'}; keySet = [0:1:7, 9, 11, 13:1:27]; colororder = [ 0.00 0 1.00 % Stab, blue 0.75 0.75 0 % Acro, yellowish gold 1.00 0 0 % alt hold, red 0.00 0 0 % auto, black 0.25 0.25 0.25 % guided 0 0.75 0.75 % loiter, teal 0.75 0.25 0.25 % RTL, brick red 0.95 0.95 0 % circle, yellow 0.25 0.25 0.75 % land 0.75 0.75 0.75 % drift 0.00 1 0 % sport, bright green 0.76 0.57 0.17 % flip, 0.75 0 0.75 % autotune, purple 0 0.5 0 % poshold, dark green 0.54 0.63 0.22 % brake 0.34 0.57 0.92 % throw 1.00 0.1 0.6 % avoid 0.88 0.75 0.73 % guided_nogps 0.10 0.49 0.47 % smart rtl 0.66 0.34 0.65 % flowhold 0.99 0.41 0.23 % follow 0.58 0.49 0.25 % zigzag 0.10 0.49 0.47 % systemid 0.66 0.34 0.65 % heli autorotate 0.99 0.41 0.23 % autortl ]; scell = cell(length(keySet),1); for kk = 1:length(keySet) eval(['s' num2str(kk) '.Col = colororder(kk,:);']) eval(['s' num2str(kk) '.Val = valueSet{kk};']) scell{kk} = eval(['s' num2str(kk)]); end if ~exist('flag','var') % modeOut = containers.Map(keySet, valueSet); modeOut = containers.Map(keySet, scell); else if flag == 0 modeOut = containers.Map(keySet, valueSet); else scell = cell(length(keySet),1); for kk = 1:length(keySet) eval(['s' num2str(kk) '.Col = colororder(kk,:);']) eval(['s' num2str(kk) '.Val = keySet(kk);']) scell{kk} = eval(['s' num2str(kk)]); end % modeOut = containers.Map(valueSet, keySet); modeOut = containers.Map(valueSet, scell); end end