-
Notifications
You must be signed in to change notification settings - Fork 0
/
mav-agent.js
executable file
·1218 lines (939 loc) · 46.4 KB
/
mav-agent.js
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#!/usr/bin/env node
//
// this is a mavlink client for ArduPlane style vehicles that has a command-line and modules,
// inspired by MAVProxy
//
// uses 'backbone.js' as a server-side Model for the Vehicle state and a group of Vehicles, and as few other dependancies as possible.
// Parts derived from some of my earlier work that had express,webserver,socketio for 'MavControl', so u might
// see spirious refs to these they haven't been removed yet.
//-------------------------------------------------------------
//
// libraries
//
//-------------------------------------------------------------
// arg handling
const yargs = require('yargs/yargs')
var argv = require('yargs/yargs')(process.argv.slice(2))
.usage('MAVAgent - Node.js and Mavlink\n\nUsage: $0 [options]')
.help('help').alias('help', 'h')
.version('version', '0.0.1').alias('version', 'V')
.options({
master: {
alias: 'm',
description: "<device-id> master serial device name. /dev/ttyUSB0 ",
requiresArg: true,
required: false
},
//out: {
// alias: 'o',
// description: "<device-id> output stream",
// requiresArg: true,
// required: false
//}
})
.argv;
//console.log('Inspecting options');
//console.dir(argv);
var master = undefined;
if (argv.master !== undefined ) {
console.log("serial master connection:", argv.master);
master = argv.master;
}
// mavlink 2 related stuff:
var {mavlink20, MAVLink20Processor} = require("./local_modules/mavlink_ardupilotmega_v2.0/mavlink.js"); //see ./local_modules/mavlink_ardupilotmega_v2.0/
var {SmartSerialLink,SmartUDPInLink,SmartUDPOutLink,SmartTCPLink,mpo} = require("./smartlinks.js");
// console.log("main",SmartSerialLink);
// console.log("main",SmartUDPInLink);
// console.log("main",SmartTCPLink);
// console.log("main",mavParserObj);
// console.log("main",mpo);
var logger = null;
var mavParserObj = mpo;
// most modules are loadable/unloadable, but these few here aren't right now.
var MavParams = require("./modules/mavParam.js"); // these are server-side js libraries for handling some more complicated bits of mavlink
var MavFlightMode = require("./modules/mavFlightMode.js");
var MavMission = require('./modules/mavMission.js');
console.log(JSON.stringify(MavFlightMode));
// config and backend libraries:
var nconf = require("nconf");
var Backbone = require("backbone");
//-------------------------------------------------------------
//
// Globals Variables, State Variables, Initialization.
//
//-------------------------------------------------------------
require('events').EventEmitter.defaultMaxListeners = 0;
// console prompt
var MODE = 'UNKNOWN';
var SYSID = ' ';
var COMPID = ' ';
// modules
var modules = {};
// self-constructing modules
var Xmodules = {};
//MAVLink20Processor is a events.EventEmitter so mavParserObj has .emit()
//----------------------------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------------------------
// empty mock objects that do nearly nothing but mean this code, designed for 'express' and socketio and 'app' doesn't have to be changed much.
// nearly event-emitter object, wich means that io.of(IONameSpace).emit(...) emits to here...
_emitterClass = function() {}
util.inherits(_emitterClass, events.EventEmitter);
_emitterClass.prototype.emit = function( msgname, msgdata) {
//console.log("EMIT!!!!:",msgname," --> " ,msgdata);
//console.log("EMIT!!!!:",msgname);
if ((msgname == 'mode') && (msgdata.mode != undefined)) {
MODE = msgdata.mode;
} // eg 'RTL'
if ((msgname == 'attitude') && (msgdata.sysid != undefined)) {
SYSID = msgdata.sysid;
COMPID = msgdata.compid;
} // eg '1'
//TIP - the messages going throu here is basically anything emitted with io.of(IONameSpace).emit(..)
// there are't mavlink msgs, they have been pre-filtered by the xxx_handler function, which
// also capture the current vehicle state into the Vehicle-Backbone object, which is kept up-to-date by them.
// emit this message to all the loaded Xmodules thatre listening..
for (const key in Xmodules ){
//console.log("Xmodule telling :", key, msgname,msgdata);
if (Xmodules[key] != undefined ) Xmodules[key].emit(msgname, msgdata);
// .. a generic message by a generic name
msgdata['name'] = msgname;
if (Xmodules[key] != undefined ) Xmodules[key].emit('message', msgdata);
}
}
var mockEmitter = new _emitterClass(); // instance of object.
// a mock io and namespace with an emit hook... so io.of(IONameSpace).emit(..) calls work, and are captured above..
var io = {}
io.on = function(event,cb) {};
io.of = function(ns) { return mockEmitter; }; // this attaches the namespace to the instance of 'mockEmitter' event emitter object just above.
var nsp = {}
nsp.on = function(event,cb) {};
//----------------------------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------------------------
// MAVControl puts most of its WebSockets into a NameSpace, so we honour that and replicate it:
IONameSpace = '/MAVControl';
//----------------------------------------------------------------------------------
var set_stream_rates = function(rate,target_system,target_component) {
// mavproxy uses a stream_rate of 4 on its links by default, so we'll just use that...
//target_system, target_component, req_stream_id, req_message_rate, start_stop
var rsr = new mavlink20.messages.request_data_stream(target_system,target_component,
mavlink20.MAV_DATA_STREAM_ALL,rate, 1);
mavParserObj.send(rsr);
console.log('Set Stream Rates =4');
}
//----------------------------------------------------------------------------------
// convenient global
var mavlinktype = undefined;
//-------------------------------------------------------------
//
// module handling
//
//-------------------------------------------------------------
function unloadModule(key,file){
console.log("->module UNloaded '"+key+"' from "+file);
// call destroy on constructed things , if it has one
if( typeof Xmodules[key].destroy !== "undefined" ) { Xmodules[key].destroy(); }
// remove constructed things
if(Xmodules.hasOwnProperty(key)) { delete Xmodules[key]; }
// remove loader references
if(modules.hasOwnProperty(key)) { delete modules[key];}
__unloadModule(file);
}
/**
* Deletes a node module and all associated children from node require cache
* @param {string} moduleName The name of the module or absolute/relative path to it
*/
function __unloadModule(moduleName) {
//console.log("->module UNloaded "+moduleName);
var solvedName = require.resolve(moduleName),
nodeModule = require.cache[solvedName];
if (nodeModule) {
for (var i = 0; i < nodeModule.children.length; i++) {
var child = nodeModule.children[i];
__unloadModule(child.filename); // recursive call
}
delete require.cache[solvedName];
}
}
//-------------------------------------------------------------
// filename = module name
function loadModule(key,file){
// already loaded?
if(modules.hasOwnProperty(key)) { return; }
// require the file, and remember it
modules[key] = require(file);
// returns a function that is a constructor directly
if( typeof modules[key] === 'function') {
//Xmodules[key] = new modules[key]();
console.log("->module loaded "+key+" from "+file);
Xmodules[key] = new modules[key](mavlink20,mavParserObj,SYSID,COMPID);
// does the module emit anything:
//Xmodules[key].on('message', (txt) => {
// console.log(txt);// buzz todo
//});
// Xmodules[key].on('LONGmessage', (txt) => {
// console.log("LONG MSG!",txt);// buzz todo
// });
}
}
function LoadModules() {
// multi-module load at startup.
var glob = require( 'glob' );
glob.sync( './modules/*.js' ).forEach( function( file ) {
let dash = file.split("/");
if(dash.length == 3) {
let dot = dash[2].split(".");
if(dot.length == 2) {
let key = dot[0];
// skip special not-quite-modules , where their file name starts with 'mav'
if (key.startsWith('mav') ) { return; }
loadModule(key,file);
}
}
});
}
//-------------------------------------------------------------
//
// serial-console user interaction like mavproxy...
//
//-------------------------------------------------------------
var ParamsObj = undefined;
var MissionObj = undefined;
// the passed in this is a Buffer of chars, we will change this to chars and words inside
process_cmdline = function(cmdline) {
// drop newline
if (cmdline[cmdline.length - 1] == 13) { cmdline.pop(); } // remove last item
var strcmd = String.fromCharCode.apply(null, cmdline); // turn array of bytes into a string
//console.log("--",strcmd,"-- "); // extra whitespace to clear rest of line
var args = strcmd.split(" "); // chars to words
// remove empty words
args = args.filter( word => word.length > 0);
// print cmd line if non-empty
if (args.length > 0 ) { console.log("--",args,"-- "); } // extra whitespace to clear rest of line
// q= quit
if (args[0] == "q") { process.exit(0); }
// a = load All modules
if (args[0] == "a") { LoadModules(); }
//-----------------------------------
if (args[0] == "ub") {
unloadModule('better','./modules/better.js');
}
// b = load 'better' module as an example
if (args[0] == "b") {
loadModule('better','./modules/better.js');
}
//----------------------------------------------------
// d = show signing debug object from signing.js, with .m and .parser and .parser.signing: MAVLinkSigning { ... } and events
if (args[0] == "d") {
console.log(mavParserObj);
}
// s = signing show internal state variable of the signing module
if (args[0] == "s") {
loadModule('signing','./modules/signing.js');
console.log(Xmodules.signing.show_state());
}
//-----------------------------
// us= unload signing
if (args[0] == "us") {
unloadModule('signing','./modules/signing.js');
}
// ss = 'signing setup' aka ss aka python self.cmd_signing_setup(args[1:])
if (args[0] == "ss") {
loadModule('signing','./modules/signing.js');
var sk = args.slice(1);
sk = "qwertyuiop";
sysid = 1;
Xmodules.signing.cmd_signing_setup([sk,sysid])
}
//-----------------------------
// lu= unload long
if (args[0] == "lu") {
unloadModule('cmdlong','./modules/cmdlong.js');
}
// ll = load long
if (args[0] == "ll") {
loadModule('cmdlong','./modules/cmdlong.js');
Xmodules.cmdlong.send('accel-cal');
//Xmodules.cmdlong.send('todo');
}
// lr = load reboot
if (args[0] == "lr") {
loadModule('cmdlong','./modules/cmdlong.js');
Xmodules.cmdlong.send('reboot');
// after a reboot we dont want the long module any more...
unloadModule('cmdlong','./modules/cmdlong.js');
}
//---------------------------
// uu = 'undo signing' aka reverse of the 'ss' command
if (args[0] == "uu") {
loadModule('signing','./modules/signing.js');
Xmodules.signing.cmd_signing_unsetup()
}
// module list
if (args[0] == "m") {
console.log("module list:" );
for (const key in Xmodules ){
console.log("\tmodule:",key );
}
}
// get all params
if (args[0] == "p") {
// objects
// MavParams are for handling loading parameters
if (ParamsObj ==undefined ) ParamsObj = new MavParams(SYSID,COMPID,mavParserObj,logger);
ParamsObj.getAll();
}
// ps = Param Show
if ((args[0] == "ps")||(args[0] == "pp")) {
// objects
// MavParams are for handling loading parameters
if (ParamsObj ==undefined ) ParamsObj = new MavParams(SYSID,COMPID,mavParserObj,logger);
var pattern = undefined;
if (args[1] != undefined) {pattern = args[1].toUpperCase();}
ParamsObj.show_fetched_params(pattern);
}
// demo retrieve one param
if (args[0] == "r") {
// objects
// MavParams are for handling loading parameters
if (ParamsObj ==undefined ) ParamsObj = new MavParams(SYSID,COMPID,mavParserObj,logger);
ParamsObj.get('SERIAL0_PROTOCOL');
}
// Send Mission to drone
if (args[0] == "sm") {
// obj for missions
if (MissionObj ==undefined ) MissionObj = new MavMission(SYSID,COMPID,mavlink20, mavParserObj , null, logger);
// load a mission in js format...
var miss = undefined;
// load by number
if (args[1] == "1") miss = require("./gotmission1.js");
if (args[1] == "2") miss = require("./gotmission2.js");
if (args[1] == "3") miss = require("./gotmission3.js");
console.log('START SEND MISSION to drone:',args[1])
// awaiting in a non-async is like this...
MissionObj.MissionToDrone(miss).then(results => { console.log('END SEND MISSION to drone'); });
}
// Get Mission from drone
if (args[0] == "gm") {
var writefilename = "./gotmission1.js"; // default if not specificed
if (args[1] == "1") writefilename = "./gotmission1.js";
if (args[1] == "2") writefilename = "./gotmission2.js";
if (args[1] == "3") writefilename = "./gotmission3.js";
// obj for missions
if (MissionObj ==undefined ) MissionObj = new MavMission(SYSID,COMPID,mavlink20, mavParserObj , null, logger);
console.log('START READ MISSION from drone')
// awaiting in a non-async is like this...
MissionObj.DroneToMission(writefilename).then(results => { console.log('END READ MISSION from drone'); });
}
// h for help, or ?
if ((args[0] == "h")||(args[0] == "?")) {
console.log("\tAvailable commands:" );
console.log("\t-------------------");
console.log("\tq - Quit" );
console.log("\th - this Help text" );
console.log("");
console.log("\td - Debug Dump of MAVLink20Processor and MAVLinkSigning objects, full stats and highlighting." );
console.log("");
console.log("\ta - load All avail modules" );
console.log("\tm - list loaded Modules" );
console.log("");
console.log("\tb - load 'Better' module, demo, etc ,it will try to ARM your vehicle and keep it armed." );
console.log("\tub - Unload 'Better' module , might want to do this before usig 'ss' " );
console.log("");
console.log("\ts - load 'Signing' module, which reports on signing events/status." );
console.log("\tss [secret] - Setup Signing, quite complex, but tries to DISARM, then activate SIGNING on this connection" );
console.log("\tuu - Deactivate Signing on this connection (uu undoes the work of ss )" );
console.log("\tus - Unload 'Signing' module" );
console.log("");
console.log("\tp - fetch all params from vehicle into local list" );
console.log("\tps [word] - show a subset of params that match the given word/pattern (eg STAT), or the entire list" );
console.log("");
console.log("\tgm [1] - Get Mission from drone and save to file ./gotmission1.js etc Works for 1,2,3" );
console.log("\tsm [1] - Send Mission to drone from local file ./gotmission1.js etc Works for 1,2,3" );
console.log("");
}
}
//-------------------------------------------------------------
// triggers 'on' even after every keypress..
// triggers 'on' even after every keypress..
if (process.stdin.isTTY) {
process.stdin.setRawMode(true);
}
var partialine = [];
process.stdin.on('readable', function () {
var data = process.stdin.read(); // data is a Buffer
if (data == null) {console.log("no data"); return;}
var key = String(data);
// emit keypress without \n
process.stdout.write(data);
partialine.push(data[0]);// build up list of keypresses.
// newline pressed on its own means, execute the command and re-show prompt
if (data[0] == 0x0d) {
process_cmdline(partialine);
partialine = [];
process.stdout.write("\nMAVAGENT-"+MODE+">"+SYSID+"+"+COMPID+"> ");
}
// backspace needs care
if (data[0] == 0x7f) {
process.stdout.write("\b"); // move cursor back, but does not clear prev pos
process.stdout.write(" "); // blank over current positon after move
process.stdout.write("\b"); // move cursor back again after blanking
// pop the backspace itself
partialine.pop();
// pop the prev char
partialine.pop();
}
// after backspacing
// console.log(data, partialine); //Buffer - use this to get hexadecimal for spedial keys
});
//-------------------------------------------------------------
// without this little block, the above 'readable' block doesnt repeat
process.stdin.on('data', function () {
var data = process.stdin.read();
return;
});
//----------------------------------------------------------------------------------
//
// first serious bit of code here.. is where we try to use either serial from --master, or tcp/udp connectors
//
//----------------------------------------------------------------------------------
// if given a serial device, try to connect to it, otehrwise we'll try to auto-connect to tcp and udpin
if (master !== undefined ) {
mpo.add_link('serial:'+master);
mpo.add_out('udpout:localhost:14550'); //to mavcontrol
//mpo.add_out('udpout:localhost:14551'); // to mission planner
} else {
console.log('--master not given. Skipping [SerialPort] and trying tcp and udp autoconnect');
console.log(' ... uplink: [tcp:localhost:5760] outlink: [udpout:localhost:14550]\n');
mpo.add_link('tcp:localhost:5760'); // to/from sitl
mpo.add_link('tcp:localhost:5770'); // to/from sitl
// mpo.add_link('tcp:localhost:5780'); // to/from sitl
// mpo.add_link('tcp:localhost:5790'); // to/from sitl
// mpo.add_link('tcp:localhost:5800'); // to/from sitl
// mpo.add_link('tcp:localhost:5810'); // to/from sitl
// mpo.add_link('tcp:localhost:5820'); // to/from sitl
// mpo.add_link('tcp:localhost:5830'); // to/from sitl
// mpo.add_link('tcp:localhost:5840'); // to/from sitl
// mpo.add_link('tcp:localhost:5850'); // to/from sitl
//mpo.add_link('udpin:blerg:14551');
//mpo.add_link('udpout:localhost:14552');
mpo.add_out('udpout:localhost:14550'); //to mavcontrol
mpo.add_out('udpout:localhost:14551'); // to mission planner
}
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
// Attach an event handler for any valid MAVLink message - we use this mostly for unknown packet types, console.log and debug messages.
// the majority of specific responses to specifc messages are not handled in the 'generic' handler, but in specific message handlers for each
// type of message. eg mavlinkParser1.on('HEATBEAT') is better than here, as this 'generic' block might go away at some point.
var generic_message_handler = function(message) {
// don't dissplay or handle parsing errors - ie Bad prefix errors, but allow signing errors thru
if ((message._id == -1 ) && (message._reason != 'Invalid signature') ) { return;}
// for packets arriving in from a --out target, their target sysid is NOT us...
if (message.target_system < 250 ) { /*console.log('--out sending:',message._name); */ mpo.send(message); }
// console.log all the uncommon message types we DONT list here.
if ( ! ['GPS_RAW_INT', 'VFR_HUD', 'ATTITUDE', 'SYS_STATUS', 'GLOBAL_POSITION_INT', 'HEARTBEAT','VIBRATION',
'BATTERY_STATUS', 'TERRAIN_REPORT', 'WIND', 'HWSTATUS', 'AHRS', 'AHRS2', 'AHRS3',
'SIMSTATE', 'RC_CHANNELS','RC_CHANNELS_RAW', 'SERVO_OUTPUT_RAW', 'LOCAL_POSITION_NED',
'MEMINFO', 'POWER_STATUS', 'SCALED_PRESSURE','SCALED_PRESSURE2', 'SCALED_IMU','SCALED_IMU2','SCALED_IMU3', 'RAW_IMU',
'EKF_STATUS_REPORT', 'SYSTEM_TIME', 'MISSION_CURRENT' , 'SENSOR_OFFSETS',
'TIMESYNC', 'PARAM_VALUE', 'HOME_POSITION', 'POSITION_TARGET_GLOBAL_INT',
'NAV_CONTROLLER_OUTPUT', 'STATUSTEXT' , 'COMMAND_ACK' ,
'MISSION_ITEM', 'MISSION_ITEM_INT','MISSION_COUNT','MISSION_REQUEST', 'MISSION_ACK',
'AIRSPEED_AUTOCAL', 'MISSION_ITEM_REACHED' , 'STAT_FLTTIME' ,'AUTOPILOT_VERSION' ,
'FENCE_STATUS' , 'AOA_SSA' , 'GPS_GLOBAL_ORIGIN', 'TERRAIN_REQUEST',
'FILE_TRANSFER_PROTOCOL', 'MOUNT_STATUS','AUTOPILOT_VERSION_REQUEST',
'REQUEST_DATA_STREAM', 'PARAM_REQUEST_READ', 'COMMAND_LONG', 'PARAM_REQUEST_LIST',
'SETUP_SIGNING', 'SET_MODE', 'MISSION_REQUEST_INT', 'FILE_TRANSFER_PROTOCOL', 'MISSION_REQUEST_LIST',
'PARAM_SET', 'TERRAIN_DATA', 'TERRAIN_CHECK',
'MISSION_SET_CURRENT',
].includes(message._name) ) {
console.log('unhandled msg type - please add it to the list....:');
console.log(message); // emit any message type that we don't list above, as we dont know about it...
}
// log PARAM_VALUE differently to exclude common ones like where param_id starts with 'STAT_RUNTIME' etc
// many of these are emitted on-boot and aren't interesting as 'normal params'
if ( ['PARAM_VALUE' ].includes(message._name) ) {
if ( message.param_id.startsWith('STAT_RUNTIME') ||
message.param_id.startsWith('STAT_FLTTIME') ||
message.param_id.startsWith('STAT_RESET') ||
message.param_id.startsWith('STAT_BOOTCNT') ||
message.param_id.startsWith('COMPASS_') ||
message.param_id.startsWith('SR0_') ||
message.param_id.startsWith('ARSPD_OFFSET') ||
message.param_id.startsWith('MIS_TOTAL') ||
message.param_id.startsWith('INS_ACC_ID') ||
message.param_id.startsWith('INS_ACC2_ID') ||
message.param_id.startsWith('INS_ACC3_ID') ||
message.param_id.startsWith('INS_GYR_ID') ||
message.param_id.startsWith('INS_GYR2_ID') ||
message.param_id.startsWith('INS_GYR3_ID') ||
message.param_id.startsWith('INS_GYROFFS_X') ||
message.param_id.startsWith('INS_GYROFFS_Y') ||
message.param_id.startsWith('INS_GYROFFS_Z') ||
message.param_id.startsWith('INS_GYR2OFFS_X') ||
message.param_id.startsWith('INS_GYR2OFFS_Y') ||
message.param_id.startsWith('INS_GYR2OFFS_Z') ||
message.param_id.startsWith('INS_GYR3OFFS_X') ||
message.param_id.startsWith('INS_GYR3OFFS_Y') ||
message.param_id.startsWith('INS_GYR3OFFS_Z') ||
message.param_id.startsWith('GND_ALT_OFFSET') ||
message.param_id.startsWith('GND_ABS_PRESS') ){
// pass
} else {
// console.log(`param fetch ${message.param_id} -> ${message.param_value} ` );
}
}
// STATUSTEXT handled elsewhere now
if ( ['COMMAND_ACK' ].includes(message._name) ) {
//console.log(`COMMAND_ACK command= ${message.command} result= ${message.result} `);
}
if ( ['MISSION_ITEM' ].includes(message._name) ) {
console.log(`MISSION_ITEM command= ${message.command} x= ${message.x} y= ${message.y} z= ${message.z} `);
}
if ( ['MISSION_ITEM_INT' ].includes(message._name) ) {
//console.log(`MISSION_ITEM_INT seq= ${message.seq} command= ${message.command} x= ${message.x} y= ${message.y} z= ${message.z} `);
//console.log(message);
}
if ( ['MISSION_COUNT' ].includes(message._name) ) {
// console.log(`MISSION_COUNT number of mission items:= ${message.count} `); //moved to mavMission.js
}
if ( ['MISSION_REQUEST' ].includes(message._name) ) {
//console.log(`MISSION_REQUEST recieved `);
}
if ( ['MISSION_ACK' ].includes(message._name) ) {
//console.log(`MISSION_ACK recieved `);
}
if ( ['MISSION_ITEM_REACHED' ].includes(message._name) ) {
console.log(`MISSION_ITEM_REACHED recieved num:= ${message.seq} `);
}
if ( ['PARAM_VALUE' ].includes(message._name) && message.param_id.startsWith('STAT_FLTTIME')){
mins = parseInt(message.param_value/60,10);
secs = parseInt(message.param_value%60,10);
console.log(`TIME IN AIR: ${mins}min:${secs}secs `);
}
}
// Attach the event handler for any valid MAVLink message in either stream, its agnostic at this stage
//mavlinkParser1.on('message', generic_message_handler);
mavParserObj.on('message', generic_message_handler);
var sysid = 12; // lets assume just one sysid to start with.
// looks for flight-mode changes on this specific sysid only
var mavFlightModes = [];
mavFlightModes.push(new MavFlightMode(mavlink20, mavParserObj, null, logger,sysid));
//-------------------------------------------------------------
//
// This is the Backbone Model that stores and accumulates all the information that is specific to a singular Vehicle.
// ( we have a group of these )
//
//-------------------------------------------------------------
// a singular aircraft
var VehicleClass = Backbone.Model.extend({
initialize: function(){
console.log("Vehicle-Backbone is initialized - sysid",this.id);
},
defaults: {
//sysid: 0 is the 'id' of this backbone obj // mavlink THIS_MAV ID of this aircraft
compid: 0, // mavlink component ID of this aircraft, set by att_handler for now.
speed: undefined, // kph. Who the hell sets this?? TODO =P
// this can likely be removed since we are most likely interested in ground speed
vehicle_type: 'Plane', // Copter or Plane as a string.
// Set by mavlink.global_position_int packets
lat: undefined,
lon: undefined,
alt: undefined,
relative_alt: undefined,
vx: undefined,
vy: undefined,
vz: undefined,
hdg: undefined,
// Set by mavlink.gps_raw_int packets
fix_type: undefined,
satellites_visible: undefined,
raw_lat: undefined,
raw_lon: undefined,
raw_alt: undefined,
eph: undefined,
epv: undefined,
vel: undefined,
cog: undefined,
// set by mavlink.attitude packets
pitch: undefined,
roll: undefined,
yaw: undefined,
pitchspeed: undefined, // acceleration
rollspeed: undefined, // acceleration
yawspeed: undefined, // acceleration
// Set by mavFlightMode interpreting mavlink.HEATBEAT etc
stateMode: undefined,
stateArmed: undefined,
// local copy
mode: undefined,
armed: undefined,
// Set by mavlink.SYS_STATUS packets
voltage_battery: undefined,
current_battery: undefined,
battery_remaining: undefined,
drop_rate_comm: undefined,
errors_comm: undefined,
// Set by mavlink.vfr_hud packets
airspeed: undefined,
groundspeed: undefined,
heading: undefined,
throttle: undefined,
climb: undefined
},
validate: function(attrs) {
attrs.lat /= 1e07;
attrs.lon /= 1e07;
attrs.alt /= 100;
}
});
//-------------------------------------------------------------
//
// this is the backbone state that has us create a "Collection" ( group ) of Vehicles, each with its own unique state,
//
//-------------------------------------------------------------
var AllVehiclesClass = Backbone.Collection.extend({
model: VehicleClass
} );
//-------------------------------------------------------------
// we instantiate a group, and just for convenience also instantiate the first vehicle in it too with a fixed/default sysid.
// ( the vehicle isn't needed to be done as it's done dynamically elsewhere )
//-------------------------------------------------------------
AllVehicles = new AllVehiclesClass();
FirstVehicle = undefined ; //new VehicleClass({id:sysid});
var __current_vehicle = undefined; //FirstVehicle; // default it to something, doesn't matter, so long as not undef
function get_current(){
return __current_vehicle = AllVehicles.get(tmp_sysid);
}
function set_current(tmp_sysid){
if ((__current_vehicle !== undefined ) && (__current_vehicle.id != tmp_sysid)) {
console.log("selected current vehicle as:"+tmp_sysid)
}
__current_vehicle = AllVehicles.get(tmp_sysid);
return __current_vehicle;
}
// put the vehicle into the collection:
AllVehicles.add(FirstVehicle);
//console.log(AllVehicles);
//console.log(FirstVehicle);
console.log("ALL:"+JSON.stringify(AllVehicles));
/*
TIP: What does _.extend do?
In simple terms, it adds properties from other objects (source) on to a target object.
Which properties will be added? Own and inherited properties which are enumerable, including those up the prototype chain.. etc
see more here: https://medium.com/@ee7klt/deconstructing-extend-492a33333079
*/
// these change/s are vehicle specific events, and so are bound to a specific vehicle in the collection, not the whole collection.
// We pull the Vehicle from the Collection by its sysid "current_vehicle = AllVehicles.get(message._header.srcSystem); "
//-------------------------------------------------------------
//
// handle all the parsed MAVLINK messages coming in to us, from UDP/tcp/serial/etc link to actual Vehicle.
//
// (the heartbeat msg is the most complex, as we use it to create new vehicles in the collection and hook state-change events.)
//
//-------------------------------------------------------------
// hooks on the 'heartbeat' right now for convenience, as its guaranteed to be on a stream..
var heartbeat_handler = function(message) {
//console.log(message);
var tmp_sysid = message._header.srcSystem;
// don't allow messages that appear to come from 255 to be hadled.
if (message._header.srcSystem == 255 ) { return; }
var current_vehicle = set_current(tmp_sysid) // returns the entire vehicle object also sets __current_vehicle global for elsewhere
// if we already have the vehicle in the collection:
if ( current_vehicle) {
//console.log("------------------------------------");
//console.log(message);
current_vehicle.set( {
last_heartbeat: Date.now(), //returns the number of milliseconds elapsed since January 1, 1970
mavlink_version: message.mavlink_version
});
io.of(IONameSpace).emit('mode', { "sysid": current_vehicle.get('id'),
"mode": current_vehicle.get('mode'),
"armed": current_vehicle.get('armed'),
"type": current_vehicle.get('vehicle_type') });
// we only CREATE new vehicle object/s when we successfully see a HEARTBEAT from them:
} else {
var tmpVehicle = new VehicleClass({id:message._header.srcSystem});
// put the modified temporary object back onto the collection
AllVehicles.add(tmpVehicle, {merge: true}); // 'add' can do "update" when merge=true, in case theres 2 of them somehow.
// assemble a new MavFlightMode hook to watch for this sysid:
mavFlightModes.push(new MavFlightMode(mavlink20, mavParserObj, null, logger,tmp_sysid));
// re-hook all the MavFlightMode objects to their respective events, since we just added a new one.
mavFlightModes.forEach( function(m) {
m.removeAllListeners('change');
//console.log("change hook mavFlightModes.length"+mavFlightModes.length);
// this event is generated locally by mavFlightMode.js, and it passed the entire 'state' AND sysid as params
m.on('change', function(state,sysid) {
// change the mode in the state subsystem to match this, but only if its changed.
var current_vehicle = AllVehicles.get(sysid);
if (current_vehicle.get('mode') != state.mode ) {
current_vehicle.set( { 'mode': state.mode});
}
if (current_vehicle.get('armed') != state.armed ) {
current_vehicle.set( { 'armed': state.armed});
}
});
});
}
}
//mavlinkParser1.on('HEARTBEAT', heartbeat_handler);
mavParserObj.on('HEARTBEAT', heartbeat_handler);
var gpi_handler = function(message) {
var current_vehicle = AllVehicles.get(message._header.srcSystem);
// if we already have the vehicle in the collection:
if ( current_vehicle) {
//console.log(`Got a GLOBAL_POSITION_INT message `);
current_vehicle.set( {
lat: message.lat / 10000000,
lon: message.lon / 10000000,
alt: message.alt / 1000,
relative_alt: message.relative_alt / 1000,
vx: message.vx / 100,
vy: message.vy / 100,
vz: message.vz / 100,
hdg: message.hdg / 100
});
//console.log("UPDATE GLOBAL_POSITION_INT:"+JSON.stringify(AllVehicles));
io.of(IONameSpace).emit('location', { "sysid": current_vehicle.get('id'),
"lat": current_vehicle.get('lat'),
"lng": current_vehicle.get('lon'),
"heading": current_vehicle.get('hdg'),
"altitude_agl": current_vehicle.get('relative_alt')});
}
}
//mavlinkParser1.on('GLOBAL_POSITION_INT', gpi_handler);
mavParserObj.on('GLOBAL_POSITION_INT', gpi_handler);
var sysstatus_handler = function(message) {
var current_vehicle = AllVehicles.get(message._header.srcSystem);
// if we already have the vehicle in the collection:
if ( current_vehicle) {
//console.log(`Got a SYS_STATUS message `);
current_vehicle.set( {
voltage_battery: message.voltage_battery,
current_battery: message.current_battery,
battery_remaining: message.battery_remaining,
drop_rate_comm: message.drop_rate_comm,
errors_comm: message.errors_comm
});
//console.log("UPDATE SYS_STATUS:"+JSON.stringify(AllVehicles));
io.of(IONameSpace).emit('sys_status', { "sysid": current_vehicle.get('id'),
"v1": current_vehicle.get('voltage_battery'),
"c1": current_vehicle.get('current_battery'),
"br": current_vehicle.get('battery_remaining'),
"drop_rate_comm": current_vehicle.get('drop_rate_comm'),
"errors_comm": current_vehicle.get('errors_comm')});
}
}
//mavlinkParser1.on('SYS_STATUS', sysstatus_handler);
mavParserObj.on('SYS_STATUS', sysstatus_handler);
// there are specific status-text messages that help us know if we really are armed/disarmed , and the rest are sent as 'message' for the web-console.
var statustext_handler = function(message) {
var current_vehicle = AllVehicles.get(message._header.srcSystem);
// if we already have the vehicle in the collection:
if ( current_vehicle) {
// drop everything including and after the first null byte.
var _message = message.text.replace(/\0.*$/g,'');
//console.log(`STATUSTEXT: ${_message}`);
// arm and disarm confirmation messages are handled like their own events, as they are important.
if (_message == "Throttle armed" || _message == "Arming motors"){
io.of(IONameSpace).emit('armed', true); // no sysid in this msg
}
if (_message == "Throttle disarmed" || _message == "Disarming motors"){
io.of(IONameSpace).emit('disarmed', true); // no sysid in this msg.
}
// everything else is just pushed into the 'messages' display box by this event...
io.of(IONameSpace).emit('status_text', { "sysid": message._header.srcSystem, "text": _message});
//io.of(IONameSpace).emit('message', { "sysid": current_vehicle.get('id'), "message": _message});
}
}
//mavlinkParser1.on('STATUSTEXT', statustext_handler);
mavParserObj.on('STATUSTEXT', statustext_handler);
var att_handler = function(message) {
//console.log(message);
if ( message._header == undefined) return;
var current_vehicle = AllVehicles.get(message._header.srcSystem);
// if we already have the vehicle in the collection:
if ( current_vehicle) {
//console.log(`Got a ATTITUDE message `);
// radians * 180.0 / 3.14159 = Angle_in_degrees
current_vehicle.set( {
pitch: Math.round(message.pitch * 180.0 / 3.14159 *100)/100,
roll: Math.round(message.roll * 180.0 / 3.14159 *100)/100,
yaw: Math.round(message.yaw * 180.0 / 3.14159 *100)/100,
pitchspeed: message.pitchspeed,
rollspeed: message.rollspeed,
yawspeed: message.yawspeed,
compid: message._header.srcComponent
});
//console.log("UPDATE ATTITUDE:"+JSON.stringify(AllVehicles));
io.of(IONameSpace).emit('attitude', { 'sysid': current_vehicle.get('id'),
'compid': current_vehicle.get('compid'),