-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathFlightControl.cpp
More file actions
1674 lines (1527 loc) · 69.3 KB
/
FlightControl.cpp
File metadata and controls
1674 lines (1527 loc) · 69.3 KB
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
#ifndef TESTING
#include "Particle.h"
#else
#include "MockParticle.h"
#endif
/*
* Project FlightControl_Demo
* Description: Core controller for Flight data logger
* Author: Bobby Schulz
* Date: 06/14/2022
* © 2023 Regents of the University of Minnesota. All rights reserved.
*/
// #define RAPID_START //Does not wait for remote connection on startup
void setup();
void loop();
void logEvents(uint8_t type, uint8_t destination);
String getErrorString();
String getDataString();
String getDiagnosticString(uint8_t level);
String getMetadataString();
String initSensors();
void quickTalonShutdown();
bool serialConnected();
void systemConfig();
int sleepSensors();
int wakeSensors();
int detectTalons(String dummyStr);
int detectSensors(String dummyStr);
int setNodeID(String nodeID);
int takeSample(String dummy);
int commandExe(String command);
int systemRestart(String resetType);
int configurePowerSave(int desiredPowerSaveMode);
int updateConfiguration(String configJson);
int getSystemConfiguration(String dummy);
int getSensorConfiguration(String dummy);
bool loadConfiguration();
void updateSensorVectors();
void initializeSensorSystem();
#define WAIT_GPS false
#define USE_CELL //System attempts to connect to cell
#include <AuxTalon.h>
#include <PCAL9535A.h>
#include <Sensor.h>
#include <Talon.h>
#include <BaroVue10.h>
#include <SDI12AnalogMux.h>
#include <Kestrel.h>
#include <KestrelFileHandler.h>
#include <Haar.h>
#include <Hedorah.h>
#include <Aleppo.h>
#include <T9602.h>
#include <SO421.h>
#include <SP421.h>
#include <TEROS11.h>
#include <ATMOS22.h>
#include <TDR315H.h>
#include <Li710.h>
#include <I2CTalon.h>
#include <SDI12Talon.h>
#include <Gonk.h>
#include <vector>
#include <memory>
#include "platform/ParticleTimeProvider.h"
#include "platform/ParticleGpio.h"
#include "platform/ParticleSystem.h"
#include "platform/ParticleWire.h"
#include "platform/ParticleCloud.h"
#include "platform/ParticleSerial.h"
#include "hardware/IOExpanderPCAL9535A.h"
#include "hardware/SDI12TalonAdapter.h"
#include "hardware/CurrentSenseAmplifierPAC1934.h"
#include "hardware/LedPCA9634.h"
#include "hardware/RtcMCP79412.h"
#include "hardware/AmbientLightVEML3328.h"
#include "hardware/GpsSFE_UBLOX_GNSS.h"
#include "hardware/HumidityTemperatureAdafruit_SHT4X.h"
#include "hardware/AccelerometerMXC6655.h"
#include "hardware/AccelerometerBMA456.h"
#include "configuration/ConfigurationManager.h"
#include "configuration/SensorManager.h"
int getIndexOfPort(int port);
const String firmwareVersion = "2.9.11";
const String schemaVersion = "2.2.9";
const unsigned long maxConnectTime = 180000; //Wait up to 180 seconds for systems to connect
const unsigned long indicatorTimeout = 60000; //Wait for up to 1 minute with indicator lights on
const uint64_t balancedDiagnosticPeriod = 3600000; //Report diagnostics once an hour //DEBUG!
int powerSaveMode = 0; //Default to 0, update when configure power save mode is called
ParticleTimeProvider realTimeProvider;
ParticleGpio realGpio;
ParticleSystem realSystem;
ParticleWire realWire;
ParticleCloud realCloud;
ParticleUSBSerial realSerialDebug;
ParticleHardwareSerial realSerialSdi12;
IOExpanderPCAL9535A realIoOB(0x20); //0x20 is the PCAL Base address
IOExpanderPCAL9535A realIoTalon(0x21);
CurrentSenseAmplifierPAC1934 realCsaAlpha(2,2,2,2,0x18);
CurrentSenseAmplifierPAC1934 realCsaBeta(2,10,10,10,0x14);
LedPCA9634 realLed(0x52);
RtcMCP79412 realRtc;
AmbientLightVEML3328 realAls;
GpsSFE_UBLOX_GNSS realGps;
HumidityTemperatureAdafruit_SHT4X realTempHumidity;
AccelerometerMXC6655 realAccel;
AccelerometerBMA456 realBackupAccel;
IOExpanderPCAL9535A ioAlpha(0x20);
IOExpanderPCAL9535A ioBeta(0x21);
Kestrel logger(realTimeProvider,
realGpio,
realSystem,
realWire,
realCloud,
realSerialDebug,
realSerialSdi12,
realIoOB,
realIoTalon,
realCsaAlpha,
realCsaBeta,
realLed,
realRtc,
realAls,
realGps,
realTempHumidity,
realAccel,
realBackupAccel,
true);
KestrelFileHandler fileSys(logger);
Gonk battery(5); //Instantiate with defaults, manually set to port 5
namespace LogModes {
constexpr uint8_t STANDARD = 0;
constexpr uint8_t PERFORMANCE = 1;
constexpr uint8_t BALANCED = 2;
constexpr uint8_t NO_LOCAL = 3; //Same as standard log, but no attempt to log to SD card
};
PRODUCT_VERSION(40)
//global variables affected by configuration manager
int backhaulCount;
unsigned long logPeriod;
int desiredPowerSaveMode;
int loggingMode;
int systemConfigUid = 0; //Used to track the UID of the configuration file
int sensorConfigUid = 0; //Used to track the UID of the sensor configuration file
String globalNodeID = ""; //Store current node ID
ConfigurationManager configManager;
SensorManager sensorManager(configManager);
std::vector<Sensor*> sensors;
std::vector<Talon*> talons;
SDI12TalonAdapter* realSdi12 = nullptr;
namespace PinsIO { //For Kestrel v1.1
constexpr uint16_t VUSB = 5;
}
namespace PinsIOAlpha {
constexpr uint16_t I2C_EXT_EN = 10;
constexpr uint16_t SD_CD = 8;
constexpr uint16_t SD_EN = 12;
constexpr uint16_t AUX_EN = 15;
constexpr uint16_t CE = 11;
constexpr uint16_t LED_EN = 13;
}
namespace PinsIOBeta { //For Kestrel v1.1
constexpr uint16_t SEL1 = 0;
constexpr uint16_t SEL2 = 4;
constexpr uint16_t SEL3 = 8;
constexpr uint16_t SEL4 = 12;
constexpr uint16_t I2C_EN1 = 1;
constexpr uint16_t I2C_EN2 = 5;
constexpr uint16_t I2C_EN3 = 9;
constexpr uint16_t I2C_EN4 = 13;
constexpr uint16_t EN1 = 3;
constexpr uint16_t EN2 = 7;
constexpr uint16_t EN3 = 11;
constexpr uint16_t EN4 = 15;
constexpr uint16_t FAULT1 = 2;
constexpr uint16_t FAULT2 = 6;
constexpr uint16_t FAULT3 = 10;
constexpr uint16_t FAULT4 = 14;
}
// SYSTEM_MODE(MANUAL); //User must call Particle.process() to stay connected to cellular after conecting, not recommended for use.
SYSTEM_MODE(SEMI_AUTOMATIC); //Particle will wait until told to connect to Cellular, but try to stay connected once connected.
// SYSTEM_MODE(AUTOMATIC); //Particle automatically tries to connect to Cellular, once connected, user code starts running.
SYSTEM_THREAD(ENABLED); //SYSTEM_THREAD enabled means Network processign runs on different thread than user loop code, recommended for use.
// SYSTEM_THREAD(DISABLED);
int detectTalons(String dummyStr = "");
int detectSensors(String dummyStr = "");
String diagnostic = "";
String errors = "";
String metadata = "";
String data = "";
void setup() {
System.enableFeature(FEATURE_RESET_INFO); //Allows for Particle to see reason for last reset using System.resetReason();
if(System.resetReason() != RESET_REASON_POWER_DOWN) {
//DEBUG! Set safe mode
Particle.connect(); //DEBUG! //If reset not caused by power switch, assume something bad happened, just connect to particle straight away
}
//////////// MANUAL POSITIONING //////////////////
// talons[aux.getTalonPort()] = &aux; //Place talon objects at coresponding positions in array
// talons[aux1.getTalonPort()] = &aux1;
time_t startTime = millis();
Particle.function("updateConfig", updateConfiguration);
Particle.function("getSystemConfig", getSystemConfiguration);
Particle.function("getSensorConfig", getSensorConfiguration);
Particle.function("nodeID", setNodeID);
Particle.function("findSensors", detectSensors);
Particle.function("findTalons", detectTalons);
Particle.function("systemRestart", systemRestart);
Particle.function("takeSample", takeSample);
Particle.function("commandExe", commandExe);
Serial.begin(1000000);
waitFor(serialConnected, 10000); //DEBUG! Wait until serial starts sending or 10 seconds
Serial.print("RESET CAUSE: "); //DEBUG!
Serial.println(System.resetReason()); //DEBUG!
bool hasCriticalError = false;
bool hasError = false;
// logger.begin(Time.now(), hasCriticalError, hasError); //Needs to be called the first time with Particle time since I2C not yet initialized
logger.begin(0, hasCriticalError, hasError); //Called with 0 since time collection system has not been initialized
Serial.println("Critial error: " + String(hasCriticalError)); //DEBUG!
Serial.println("Error: " + String(hasError)); //DEBUG!
logger.setIndicatorState(IndicatorLight::ALL,IndicatorMode::INIT);
bool batState = logger.testForBat(); //Check if a battery is connected
logger.enableI2C_OB(false);
logger.enableI2C_External(true); //Connect to Gonk I2C port
logger.enableI2C_Global(true);
if(batState) battery.setIndicatorState(GonkIndicatorMode::SOLID); //Turn on charge indication LEDs during setup
else battery.setIndicatorState(GonkIndicatorMode::BLINKING); //If battery not switched on, set to blinking
fileSys.begin(0, hasCriticalError, hasError); //Initialzie, but do not attempt backhaul
Serial.println("Critial error: " + String(hasCriticalError)); //DEBUG!
Serial.println("Error: " + String(hasError)); //DEBUG!
if(hasCriticalError) {
Serial.println(getErrorString()); //Report current error codes
logger.setIndicatorState(IndicatorLight::STAT,IndicatorMode::ERROR); //Display error state if critical error is reported
}
else logger.setIndicatorState(IndicatorLight::STAT,IndicatorMode::PASS); //If no critical fault, switch STAT off
battery.begin(0, hasCriticalError, hasError); //Init final CORE element
// I2C_OnBoardEn(true);
// Wire.setClock(400000); //Confirm operation in fast mode
// Wire.begin();
logger.enableI2C_Global(false); //Connect to internal bus
logger.enableI2C_OB(true);
ioAlpha.begin(); //RESTORE
ioBeta.begin(); //RESTORE
// ioBeta.pinMode(PinsIOBeta::SEL2, OUTPUT); //DEBUG
// ioBeta.digitalWrite(PinsIOBeta::SEL2, LOW); //DEBUG
ioAlpha.pinMode(PinsIOAlpha::LED_EN, OUTPUT);
ioAlpha.digitalWrite(PinsIOAlpha::LED_EN, LOW); //Turn on LED indicators
// logger.setIndicatorState(IndicatorLight::ALL,IndicatorMode::IDLE);
// waitFor(serialConnected, 10000); //DEBUG! Wait until serial starts sending or 10 seconds
if(Serial.available()) {
//COMMAND MODE!
logger.setIndicatorState(IndicatorLight::ALL,IndicatorMode::COMMAND);
systemConfig();
}
logger.setIndicatorState(IndicatorLight::ALL,IndicatorMode::WAITING);
Particle.connect(); //Once passed attempted serial connect, try to connect to particle
////////// ADD INTERRUPTS!
// for(int i = 1; i <= Kestrel::numTalonPorts; i++) { //Iterate over ALL ports
// logger.enablePower(i, true); //Turn on all power by default
// logger.enablePower(i, false); //Toggle power to reset
// logger.enablePower(i, true);
// logger.enableData(i, false); //Turn off all data by default
// }
//load configuration from SD card, default config if not found or not possible
Serial.println("Loading configuration..."); //DEBUG!
loadConfiguration();
Serial.println("Configuration loaded"); //DEBUG!
//initilize all sensors
Serial.println("Initializing sensors..."); //DEBUG!
initializeSensorSystem();
Serial.println("Sensors initialized"); //DEBUG!
// Apply power save mode
configurePowerSave(desiredPowerSaveMode); //Setup power mode of the system (Talons and Sensors)
detectTalons();
detectSensors();
// I2C_OnBoardEn(false);
// I2C_GlobalEn(true);
// bool hasCriticalError = false;
// bool hasError = false;
// logger.enableData(4, true);
// logger.enableI2C_OB(false);
// logger.enableI2C_Global(true);
// String initDiagnostic = aux.begin(Time.now(), hasCriticalError, hasError);
logger.updateLocation(true); //Force GPS update (hopfully connected)
String initDiagnostic = initSensors();
Serial.print("DIAGNOSTIC: ");
Serial.println(initDiagnostic);
if(loggingMode == LogModes::NO_LOCAL) {
fileSys.writeToFRAM(initDiagnostic, DataType::Diagnostic, DestCodes::Particle);
logEvents(4, DestCodes::Particle); //Grab data log with metadata, no diagnostics
}
else {
fileSys.writeToFRAM(initDiagnostic, DataType::Diagnostic, DestCodes::Both);
logEvents(4, DestCodes::Both); //Grab data log with metadata, no diagnostics
}
// fileSys.writeToParticle(initDiagnostic, "diagnostic");
// // logger.enableSD(true);
// fileSys.writeToSD(initDiagnostic, "Dummy.txt");
#ifndef RAPID_START //Only do this if not rapid starting
while((!Particle.connected() || logger.m_gps.getFixType() == 0) && (millis() - startTime) < maxConnectTime) { //Wait while at least one of the remote systems is not connected
if(Particle.connected()) {
logger.setIndicatorState(IndicatorLight::CELL, IndicatorMode::PASS); //If cell is connected, set to PASS state
if(WAIT_GPS == false) break; //If not told to wait for GPS, break out after cell is connected
}
if(logger.m_gps.getTimeValid() == true) {
if(logger.m_gps.getFixType() >= 2 && logger.m_gps.getFixType() <= 4 && logger.m_gps.getGnssFixOk()) { //If you get a 2D fix or better, pass GPS
logger.setIndicatorState(IndicatorLight::GPS, IndicatorMode::PASS);
}
else {
logger.setIndicatorState(IndicatorLight::GPS, IndicatorMode::PREPASS); //If time is good, set preliminary pass only
}
}
Serial.println("Wait for cell connect..."); //DEBUG!
delay(5000); //Wait 5 seconds between each check to not lock up the process //DEBUG!
}
#endif
if(Particle.connected()) logger.setIndicatorState(IndicatorLight::CELL, IndicatorMode::PASS); //Catches connection of cell is second device to connect
else {
logger.setIndicatorState(IndicatorLight::CELL, IndicatorMode::ERROR); //If cell still not connected, display error
// Particle.disconnect(); //DEBUG!
}
if(logger.m_gps.getFixType() >= 2 && logger.m_gps.getFixType() <= 4 && logger.m_gps.getGnssFixOk()) { //Make fix report is in range and fix is OK
logger.setIndicatorState(IndicatorLight::GPS, IndicatorMode::PASS); //Catches connection of GPS is second device to connect
}
else {
logger.setIndicatorState(IndicatorLight::GPS, IndicatorMode::ERROR); //If GPS fails to connect after period, set back to error
}
fileSys.tryBackhaul(); //See if we can backhaul any unsent logs
// fileSys.writeToFRAM(getDiagnosticString(1), DataType::Diagnostic, DestCodes::Both); //DEBUG!
// logEvents(3); //Grab data log with metadata //DEBUG!
fileSys.dumpFRAM(); //Backhaul this data right away
// Particle.publish("diagnostic", initDiagnostic);
// logger.enableData(3, true);
// logger.enableI2C_OB(false);
// logger.enableI2C_Global(true);
// aux1.begin(Time.now(), hasCriticalError, hasError);
//FIX! RESPOND TO ERROR RESULTS!
}
void loop() {
// aux.sleep(false);
static int count = 1; //Keep track of number of cycles
logger.wake(); //Wake up logger system
fileSys.wake(); //Wake up file handling
wakeSensors(); //Wake each sensor
if(System.millis() > indicatorTimeout) {
logger.setIndicatorState(IndicatorLight::ALL, IndicatorMode::NONE); //Turn LED indicators off if it has been longer than timeout since startup (use system.millis() which does not rollover)
logger.enableI2C_OB(false);
logger.enableI2C_External(true); //Connect to Gonk I2C port
logger.enableI2C_Global(true);
battery.setIndicatorState(GonkIndicatorMode::PUSH_BUTTON); //Turn off indicator lights on battery, return to push button control
logger.enableI2C_External(false); //Turn off external I2C
}
bool alarm = logger.waitUntilTimerDone(); //Wait until the timer period has finished //REPLACE FOR NON-SLEEP
// if(alarm) Serial.println("RTC Wakeup"); //DEBUG!
// else Serial.println("Timeout Wakeup"); //DEBUG!
// Serial.print("RAM, Start Log Events: "); //DEBUG!
// Serial.println(System.freeMemory()); //DEBUG!
logger.startTimer(logPeriod); //Start timer as soon done reading sensors //REPLACE FOR NON-SLEEP
switch(loggingMode) {
static uint64_t lastDiagnostic = System.millis();
case (LogModes::PERFORMANCE):
logEvents(6, DestCodes::Both);
break;
case (LogModes::STANDARD):
if((count % 16) == 0) logEvents(3, DestCodes::Both);
else if((count % 8) == 0) logEvents(2, DestCodes::Both);
else if((count % 1) == 0) logEvents(1, DestCodes::Both);
break;
case (LogModes::BALANCED):
logEvents(7, DestCodes::Both);
if((System.millis() - lastDiagnostic) > balancedDiagnosticPeriod) {
logEvents(3, DestCodes::Both); //Do a full diagnostic and metadata report once an hour
lastDiagnostic = System.millis();
}
break;
case (LogModes::NO_LOCAL):
if((count % 10) == 0) logEvents(3, DestCodes::Particle);
else if((count % 5) == 0) logEvents(2, DestCodes::Particle);
else if((count % 1) == 0) logEvents(1, DestCodes::Particle);
break;
default:
logEvents(1, DestCodes::Both); //If unknown configuration, use general call
// break;
}
// Serial.print("RAM, End Log Events: "); //DEBUG!
// Serial.println(System.freeMemory()); //DEBUG!
Serial.println("Log Done"); //DEBUG!
Serial.print("WDT Status: "); //DEBUG!
Serial.println(logger.feedWDT());
sleepSensors();
// Particle.publish("diagnostic", diagnostic);
// Particle.publish("error", errors);
// Particle.publish("data", data);
// Particle.publish("metadata", metadata);
// Serial.print("DIAGNOSTIC: ");
// Serial.println(diagnostic);
// Serial.print("ERROR: ");
// Serial.println(errors);
// Serial.print("DATA: ");
// Serial.println(data);
// Serial.print("METADATA: ");
// Serial.println(metadata);
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
// fileSys.writeToFRAM(diagnostic, "diagnostic", DestCodes::Particle);
if((count % backhaulCount) == 0) {
Serial.println("BACKHAUL"); //DEBUG!
if(powerSaveMode >= PowerSaveModes::LOW_POWER) {
Particle.connect();
waitFor(Particle.connected, 300000); //Wait up to 5 minutes to connect if using low power modes
}
logger.syncTime();
fileSys.dumpFRAM(); //dump FRAM every Nth log
}
count++;
fileSys.sleep(); //Wait to sleep until after backhaul attempt
logger.sleep(); //Put system into sleep mode
// SystemSleepConfiguration config;
// config.mode(SystemSleepMode::STOP)
// .network(NETWORK_INTERFACE_CELLULAR)
// .flag(SystemSleepFlag::WAIT_CLOUD)
// .duration(2min);
// System.sleep(config);
}
void logEvents(uint8_t type, uint8_t destination)
{
// String diagnostic = "";
// String errors = "";
// String metadata = "";
// String data = "";
diagnostic = "";
errors = "";
metadata = "";
data = "";
Serial.print("LOG: "); //DEBUG!
Serial.println(type);
if(type == 0) { //Grab errors only
// data = getDataString();
// diagnostic = getDiagnosticString(4); //DEBUG! RESTORE
errors = getErrorString(); //Get errors last to wait for error codes to be updated //DEBUG! RESTORE
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
Serial.println(errors); //DEBUG!
// Serial.println(data); //DEBUG!
// Serial.println(diagnostic); //DEBUG!
if(errors.equals("") == false) {
// Serial.println("Write Errors to FRAM"); //DEBUG!
fileSys.writeToFRAM(errors, DataType::Error, destination); //Write value out only if errors are reported
}
// fileSys.writeToFRAM(data, DataType::Data, DestCodes::Both);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::Both);
}
if(type == 1) {
data = getDataString();
diagnostic = getDiagnosticString(4); //DEBUG! RESTORE
errors = getErrorString(); //Get errors last to wait for error codes to be updated //DEBUG! RESTORE
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
Serial.println(errors); //DEBUG!
Serial.println(data); //DEBUG!
Serial.println(diagnostic); //DEBUG!
if(errors.equals("") == false) {
// Serial.println("Write Errors to FRAM"); //DEBUG!
fileSys.writeToFRAM(errors, DataType::Error, destination); //Write value out only if errors are reported
}
fileSys.writeToFRAM(data, DataType::Data, destination);
fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, destination);
}
else if(type == 2) {
data = getDataString();
diagnostic = getDiagnosticString(3);
errors = getErrorString();
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, destination); //Write value out only if errors are reported
fileSys.writeToFRAM(data, DataType::Data, destination);
fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, destination);
}
else if(type == 3) {
data = getDataString();
diagnostic = getDiagnosticString(2);
metadata = getMetadataString();
errors = getErrorString();
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, destination); //Write value out only if errors are reported
fileSys.writeToFRAM(data, DataType::Data, destination);
fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, destination);
fileSys.writeToFRAM(metadata, DataType::Metadata, destination);
}
else if(type == 4) { //To be used on startup, don't grab diagnostics since init already got them
data = getDataString();
// diagnostic = getDiagnosticString(2);
metadata = getMetadataString();
errors = getErrorString();
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
Serial.println(errors); //DEBUG!
Serial.println(data); //DEBUG
Serial.println(metadata); //DEBUG!
if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, destination); //Write value out only if errors are reported
fileSys.writeToFRAM(data, DataType::Data, destination);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::Both);
fileSys.writeToFRAM(metadata, DataType::Metadata, destination);
}
else if(type == 5) { //To be used on startup, don't grab diagnostics since init already got them
data = getDataString();
diagnostic = getDiagnosticString(5);
// metadata = getMetadataString();
errors = getErrorString();
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
Serial.println(errors); //DEBUG!
Serial.println(data); //DEBUG
// Serial.println(metadata); //DEBUG!
if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, DestCodes::SD); //Write value out only if errors are reported
fileSys.writeToFRAM(data, DataType::Data, DestCodes::SD);
fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::SD);
// fileSys.writeToFRAM(metadata, DataType::Metadata, DestCodes::Both);
}
else if(type == 6) { //Log ONLY data - fastest method
data = getDataString();
// diagnostic = getDiagnosticString(5);
// metadata = getMetadataString();
// errors = getErrorString();
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
// Serial.println(errors); //DEBUG!
// Serial.println(data); //DEBUG
// Serial.println(metadata); //DEBUG!
// if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, DestCodes::SD); //Write value out only if errors are reported
fileSys.writeToFRAM(data, DataType::Data, destination);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::SD);
// fileSys.writeToFRAM(metadata, DataType::Metadata, DestCodes::Both);
}
else if(type == 7) { //Log data and error if there
data = getDataString();
// diagnostic = getDiagnosticString(5);
// metadata = getMetadataString();
errors = getErrorString();
// logger.enableI2C_OB(true);
// logger.enableI2C_Global(false);
// Serial.println(errors); //DEBUG!
// Serial.println(data); //DEBUG
// Serial.println(metadata); //DEBUG!
if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, destination); //Write value out only if errors are reported
fileSys.writeToFRAM(data, DataType::Data, destination);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::SD);
// fileSys.writeToFRAM(metadata, DataType::Metadata, DestCodes::Both);
}
// switch(type) {
// case 1: //Standard, short interval, log
// data = getDataString();
// diagnostic = getDiagnosticString(4); //DEBUG! RESTORE
// errors = getErrorString(); //Get errors last to wait for error codes to be updated //DEBUG! RESTORE
// // logger.enableI2C_OB(true);
// // logger.enableI2C_Global(false);
// Serial.println(errors); //DEBUG!
// Serial.println(data); //DEBUG!
// Serial.println(diagnostic); //DEBUG!
// if(errors.equals("") == false) {
// // Serial.println("Write Errors to FRAM"); //DEBUG!
// fileSys.writeToFRAM(errors, DataType::Error, DestCodes::Both); //Write value out only if errors are reported
// }
// fileSys.writeToFRAM(data, DataType::Data, DestCodes::Both);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::Both);
// break;
// case 2: //Low period log with diagnostics
// data = getDataString();
// diagnostic = getDiagnosticString(3);
// errors = getErrorString();
// // logger.enableI2C_OB(true);
// // logger.enableI2C_Global(false);
// if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, DestCodes::Both); //Write value out only if errors are reported
// fileSys.writeToFRAM(data, DataType::Data, DestCodes::Both);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::Both);
// break;
// case 3: //Daily log event with increased diagnostics and metadata
// data = getDataString();
// diagnostic = getDiagnosticString(2);
// metadata = getMetadataString();
// errors = getErrorString();
// // logger.enableI2C_OB(true);
// // logger.enableI2C_Global(false);
// if(errors.equals("") == false) fileSys.writeToFRAM(errors, DataType::Error, DestCodes::Both); //Write value out only if errors are reported
// fileSys.writeToFRAM(data, DataType::Data, DestCodes::Both);
// fileSys.writeToFRAM(diagnostic, DataType::Diagnostic, DestCodes::Both);
// fileSys.writeToFRAM(metadata, DataType::Metadata, DestCodes::Both);
// break;
// }
// diagnostic* = (const char*)NULL;
// data* = (const char*)NULL;
// metadata* = (const char*)NULL;
// errors* = (const char*)NULL;
}
String getErrorString()
{
unsigned long numErrors = 0; //Used to keep track of total errors across all devices
String errors = "{\"Error\":{";
errors = errors + "\"Time\":" + logger.getTimeString() + ","; //Concatonate time
errors = errors + "\"Loc\":[" + logger.getPosLat() + "," + logger.getPosLong() + "," + logger.getPosAlt() + "," + logger.getPosTimeString() + "],";
if(globalNodeID != "") errors = errors + "\"Node ID\":\"" + globalNodeID + "\","; //Concatonate node ID
else errors = errors + "\"Device ID\":\"" + System.deviceID() + "\","; //If node ID not initialized, use device ID
errors = errors + "\"Packet ID\":" + logger.getMessageID() + ","; //Concatonate unique packet hash
errors = errors + "\"NumDevices\":" + String(sensors.size()) + ","; //Concatonate number of sensors
errors = errors + "\"Devices\":[";
for(int i = 0; i < sensors.size(); i++) {
if(sensors[i]->totalErrors() > 0) {
numErrors = numErrors + sensors[i]->totalErrors(); //Increment the total error count
if(!errors.endsWith("[")) errors = errors + ","; //Only append if not first entry
errors = errors + "{" + sensors[i]->getErrors() + "}";
}
}
errors = errors + "]}}"; //Close data
Serial.print("Num Errors: "); //DEBUG!
Serial.println(numErrors);
if(numErrors > 0) return errors;
else return ""; //Return null string if no errors reported
}
String getDataString()
{
String leader = "{\"Data\":{";
leader = leader + "\"Time\":" + logger.getTimeString() + ","; //Concatonate time
leader = leader + "\"Loc\":[" + logger.getPosLat() + "," + logger.getPosLong() + "," + logger.getPosAlt() + "," + logger.getPosTimeString() + "],";
if(globalNodeID != "") leader = leader + "\"Node ID\":\"" + globalNodeID + "\","; //Concatonate node ID
else leader = leader + "\"Device ID\":\"" + System.deviceID() + "\","; //If node ID not initialized, use device ID
leader = leader + "\"Packet ID\":" + logger.getMessageID() + ","; //Concatonate unique packet hash
leader = leader + "\"NumDevices\":" + String(sensors.size()) + ","; //Concatonate number of sensors
leader = leader + "\"Devices\":[";
const String closer = "]}}";
String output = leader;
uint8_t deviceCount = 0; //Used to keep track of how many devices have been appended
for(int i = 0; i < sensors.size(); i++) {
logger.disableDataAll(); //Turn off data to all ports, then just enable those needed
if(sensors[i]->sensorInterface != BusType::CORE && sensors[i]->getTalonPort() != 0) {
logger.enablePower(sensors[i]->getTalonPort(), true); //Turn on kestrel port for needed Talon, only if not core system and port is valid
Serial.println("Enabled power for Talon: " + String(sensors[i]->getTalonPort())); //DEBUG!
}
if(sensors[i]->sensorInterface != BusType::CORE && sensors[i]->getTalonPort() != 0) {
logger.enableData(sensors[i]->getTalonPort(), true); //Turn on kestrel port for needed Talon, only if not core system and port is valid
Serial.println("Enabled data for Talon: " + String(sensors[i]->getTalonPort())); //DEBUG!
}
logger.enableI2C_OB(false);
logger.enableI2C_Global(true);
bool dummy1;
bool dummy2;
int currentTalonIndex = getIndexOfPort(sensors[i]->getTalonPort()); //Find the talon associated with this sensor
if((sensors[i]->getTalonPort() > 0) && (currentTalonIndex >= 0)) { //DEBUG! REPALCE!
Serial.print("TALON CALL: "); //DEBUG!
Serial.println(sensors[i]->getTalonPort());
logger.configTalonSense(); //Setup to allow for current testing
// talons[sensors[i]->getTalonPort() - 1]->begin(logger.getTime(), dummy1, dummy2); //DEBUG! Do only if talon is associated with sensor, and object exists
talons[currentTalonIndex]->restart(); //DEBUG! Do only if talon is associated with sensor, and object exists
// logger.enableI2C_OB(false); //Return to isolation mode
// logger.enableI2C_Global(true);
}
if(sensors[i]->getSensorPort() > 0 && sensors[i]->getTalonPort() > 0) { //If not a Talon
Serial.print("Device "); //DEBUG!
Serial.print(i);
Serial.println(" is a sensor");
if(currentTalonIndex >= 0) { //DEBUG! REPALCE!
talons[currentTalonIndex]->disableDataAll(); //Turn off all data ports to start for the given Talon
// talons[sensors[i]->getTalonPort() - 1]->disablePowerAll(); //Turn off all power ports to start for the given Talon
// talons[sensors[i]->getTalonPort() - 1]->enablePower(sensors[i]->getSensorPort(), true); //Turn on power for the given port on the Talon
talons[currentTalonIndex]->enableData(sensors[i]->getSensorPort(), true); //Turn on data for the given port on the Talon
// bool dummy1;
// bool dummy2;
// sensors[i]->begin(Time.now(), dummy1, dummy2); //DEBUG!
}
}
// delay(100); //DEBUG!
logger.enableI2C_OB(false);
logger.enableI2C_Global(true);
Serial.print("Data string from sensor "); //DEBUG!
Serial.print(i);
Serial.print(": ");
String val = sensors[i]->getData(logger.getTime());
Serial.println(val);
if(!val.equals("")) { //Only append if not empty string
if(output.length() - output.lastIndexOf('\n') + val.length() + closer.length() + 1 < Kestrel::MAX_MESSAGE_LENGTH) { //Add +1 to account for comma appending, subtract any previous lines from count
if(deviceCount > 0) output = output + ","; //Add preceeding comma if not the first entry
output = output + "{" + val + "}"; //Append result
deviceCount++;
// if(i + 1 < sensors.size()) diagnostic = diagnostic + ","; //Only append if not last entry
}
else {
output = output + closer + "\n"; //End this packet
output = output + leader + "{" + val + "}"; //Start a new packet and add new payload
}
}
// if(!val.equals("")) { //Only append if real result
// if(deviceCount > 0) data = data + ","; //Preappend comma only if not first addition
// data = data + "{" + val + "}";
// deviceCount++;
// // if(i + 1 < sensors.size()) metadata = metadata + ","; //Only append if not last entry
// }
Serial.print("Cumulative data string: "); //DEBUG!
Serial.println(output); //DEBUG!
// data = data + sensors[i]->getData(logger.getTime()); //DEBUG! REPLACE!
// if(i + 1 < sensors.size()) data = data + ","; //Only append if not last entry
if((sensors[i]->getSensorPort() > 0) && (sensors[i]->getTalonPort() > 0) && (currentTalonIndex >= 0)) {
talons[currentTalonIndex]->enableData(sensors[i]->getSensorPort(), false); //Turn off data for the given port on the Talon
// talons[sensors[i]->getTalonPort() - 1]->enablePower(sensors[i]->getSensorPort(), false); //Turn off power for the given port on the Talon //DEBUG!
}
}
output = output + "]}}"; //Close data
return output;
}
String getDiagnosticString(uint8_t level)
{
String leader = "{\"Diagnostic\":{";
leader = leader + "\"Time\":" + logger.getTimeString() + ","; //Concatonate time
leader = leader + "\"Loc\":[" + logger.getPosLat() + "," + logger.getPosLong() + "," + logger.getPosAlt() + "," + logger.getPosTimeString() + "],";
if(globalNodeID != "") leader = leader + "\"Node ID\":\"" + globalNodeID + "\","; //Concatonate node ID
else leader = leader + "\"Device ID\":\"" + System.deviceID() + "\","; //If node ID not initialized, use device ID
leader = leader + "\"Packet ID\":" + logger.getMessageID() + ","; //Concatonate unique packet hash
leader = leader + "\"NumDevices\":" + String(sensors.size()) + ",\"Level\":" + String(level) + ",\"Devices\":["; //Concatonate number of sensors and level
const String closer = "]}}";
String output = leader;
uint8_t deviceCount = 0; //Used to keep track of how many devices have been appended
for(int i = 0; i < sensors.size(); i++) {
logger.disableDataAll(); //Turn off data to all ports, then just enable those needed
if(sensors[i]->sensorInterface != BusType::CORE && sensors[i]->getTalonPort() != 0) logger.enablePower(sensors[i]->getTalonPort(), true); //Turn on kestrel port for needed Talon, only if not core system and port is valid
if(sensors[i]->sensorInterface != BusType::CORE && sensors[i]->getTalonPort() != 0) logger.enableData(sensors[i]->getTalonPort(), true); //Turn on kestrel port for needed Talon, only if not core system and port is valid
logger.enableI2C_OB(false);
logger.enableI2C_Global(true);
// if(!sensors[i]->isTalon()) { //If sensor is not Talon
int currentTalonIndex = getIndexOfPort(sensors[i]->getTalonPort()); //Find the talon associated with this sensor
if((sensors[i]->getSensorPort() > 0) && (sensors[i]->getTalonPort() > 0) && (currentTalonIndex >= 0)) { //If a Talon is associated with the sensor, turn that port on
talons[currentTalonIndex]->disableDataAll(); //Turn off all data on Talon
// talons[sensors[i]->getTalonPort() - 1]->enablePower(sensors[i]->getSensorPort(), true); //Turn on power for the given port on the Talon
talons[currentTalonIndex]->enableData(sensors[i]->getSensorPort(), true); //Turn back on only port used
}
String diagnostic = sensors[i]->selfDiagnostic(level, logger.getTime());
if(!diagnostic.equals("")) { //Only append if not empty string
if(output.length() - output.lastIndexOf('\n') + diagnostic.length() + closer.length() + 1 < Kestrel::MAX_MESSAGE_LENGTH) { //Add +1 to account for comma appending, subtract any previous lines from count
if(deviceCount > 0) output = output + ","; //Add preceeding comma if not the first entry
output = output + "{" + diagnostic + "}"; //Append result
deviceCount++;
// if(i + 1 < sensors.size()) diagnostic = diagnostic + ","; //Only append if not last entry
}
else {
output = output + closer + "\n"; //End this packet
output = output + leader + "{" + diagnostic + "}"; //Start a new packet and add new payload
}
}
if((sensors[i]->getSensorPort() > 0) && (sensors[i]->getTalonPort() > 0) && (currentTalonIndex >= 0)) {
talons[currentTalonIndex]->enableData(sensors[i]->getSensorPort(), false); //Turn off data for the given port on the Talon
// talons[sensors[i]->getTalonPort() - 1]->enablePower(sensors[i]->getSensorPort(), false); //Turn off power for the given port on the Talon //DEBUG!
}
}
output = output + closer; //Close diagnostic
return output;
}
String getMetadataString()
{
String leader = "{\"Metadata\":{";
leader = leader + "\"Time\":" + logger.getTimeString() + ","; //Concatonate time
leader = leader + "\"Loc\":[" + logger.getPosLat() + "," + logger.getPosLong() + "," + logger.getPosAlt() + "," + logger.getPosTimeString() + "],";
if(globalNodeID != "") leader = leader + "\"Node ID\":\"" + globalNodeID + "\","; //Concatonate node ID
else leader = leader + "\"Device ID\":\"" + System.deviceID() + "\","; //If node ID not initialized, use device ID
leader = leader + "\"Packet ID\":" + logger.getMessageID() + ","; //Concatonate unique packet hash
leader = leader + "\"NumDevices\":" + String(sensors.size()) + ","; //Concatonate number of sensors
leader = leader + "\"Devices\":[";
const String closer = "]}}";
String output = leader;
output = output + "{\"System\":{";
// output = output + "\"DUMMY\":\"BLOODYMARYBLOODYMARYBLODDYMARY\",";
output = output + "\"Schema\":\"" + schemaVersion + "\",";
output = output + "\"Firm\":\"" + firmwareVersion + "\",";
output = output + "\"OS\":\"" + System.version() + "\",";
output = output + "\"ID\":\"" + System.deviceID() + "\",";
output = output + "\"Update\":" + String(logPeriod) + ",";
output = output + "\"Backhaul\":" + String(backhaulCount) + ",";
output = output + "\"LogMode\":" + String(loggingMode) + ",";
output = output + "\"Sleep\":" + String(powerSaveMode) + ",";
output = output + "\"SysConfigUID\":" + String(configManager.updateSystemConfigurationUid()) + ",";
output = output + "\"SensorConfigUID\":" + String(configManager.updateSensorConfigurationUid()) + "}},";
//FIX! Add support for device name
uint8_t deviceCount = 0; //Used to keep track of how many devices have been appended
for(int i = 0; i < sensors.size(); i++) {
logger.disableDataAll(); //Turn off data to all ports, then just enable those needed
if(sensors[i]->sensorInterface != BusType::CORE && sensors[i]->getTalonPort() != 0) logger.enableData(sensors[i]->getTalonPort(), true); //Turn on data to required Talon port only if not core and port is valid
// if(!sensors[i]->isTalon()) { //If sensor is not Talon
int currentTalonIndex = getIndexOfPort(sensors[i]->getTalonPort()); //Find the talon associated with this sensor
if((sensors[i]->getSensorPort() > 0) && (sensors[i]->getTalonPort() > 0) && (currentTalonIndex >= 0)) { //If a Talon is associated with the sensor, turn that port on
talons[currentTalonIndex]->disableDataAll(); //Turn off all data on Talon
talons[currentTalonIndex]->enableData(sensors[i]->getSensorPort(), true); //Turn back on only port used
}
// logger.enablePower(sensors[i]->getTalon(), true); //Turn on power to port
// logger.enableData(sensors[i]->getTalon(), true); //Turn on data to port
logger.enableI2C_OB(false);
logger.enableI2C_Global(true);
String val = sensors[i]->getMetadata();
// metadata = metadata + sensors[i]->getMetadata();
// if(!val.equals("")) { //Only append if real result
// if(deviceCount > 0) metadata = metadata + ","; //Preappend comma only if not first addition
// metadata = metadata + val;
// deviceCount++;
// // if(i + 1 < sensors.size()) metadata = metadata + ","; //Only append if not last entry
// }
if(!val.equals("")) { //Only append if not empty string
if(output.length() - output.lastIndexOf('\n') + val.length() + closer.length() + 1 < Kestrel::MAX_MESSAGE_LENGTH) { //Add +1 to account for comma appending, subtract any previous lines from count
if(deviceCount > 0) output = output + ","; //Add preceeding comma if not the first entry
output = output + "{" + val + "}"; //Append result
deviceCount++;
// if(i + 1 < sensors.size()) diagnostic = diagnostic + ","; //Only append if not last entry
}
else {
output = output + closer + "\n"; //End this packet
output = output + leader + "{" + val + "}"; //Start a new packet and add new payload
}
}
}
output = output + closer; //Close metadata
return output;
}
String initSensors()
{
String leader = "{\"Diagnostic\":{";
leader = leader + "\"Time\":" + logger.getTimeString() + ","; //Concatonate time
leader = leader + "\"Loc\":[" + logger.getPosLat() + "," + logger.getPosLong() + "," + logger.getPosAlt() + "," + logger.getPosTimeString() + "],";
if(globalNodeID != "") leader = leader + "\"Node ID\":\"" + globalNodeID + "\","; //Concatonate node ID
else leader = leader + "\"Device ID\":\"" + System.deviceID() + "\","; //If node ID not initialized, use device ID
leader = leader + "\"Packet ID\":" + logger.getMessageID() + ","; //Concatonate unique packet hash
leader = leader + "\"NumDevices\":" + String(sensors.size()) + ",\"Devices\":["; //Concatonate number of sensors and level
String closer = "]}}";
String output = leader;
bool reportCriticalError = false; //Used to keep track of the global status of the error indications for all sensors
bool reportError = false;
bool missingSensor = false;
// output = output + "\"Devices\":[";
uint8_t deviceCount = 0; //Used to keep track of how many devices have been appended
for(int i = 0; i < sensors.size(); i++) {
logger.disableDataAll(); //Turn off data to all ports, then just enable those needed
if(sensors[i]->sensorInterface != BusType::CORE && sensors[i]->getTalonPort() != 0) logger.enableData(sensors[i]->getTalonPort(), true); //Turn on data to required Talon port only if not core and the port is valid
logger.enableI2C_OB(false);
logger.enableI2C_Global(true);
bool dummy1;
bool dummy2;
// if(!sensors[i]->isTalon()) { //If sensor is not Talon
logger.configTalonSense(); //Setup to allow for current testing
// if(sensors[i]->getTalonPort() > 0 && talons[sensors[i]->getTalonPort() - 1]) talons[sensors[i]->getTalonPort() - 1]->begin(logger.getTime(), dummy1, dummy2); //DEBUG! Do only if talon is associated with sensor, and object exists //DEBUG! REPLACE!
int currentTalonIndex = getIndexOfPort(sensors[i]->getTalonPort());
if(currentTalonIndex >= 0)
{
if(sensors[i]->getTalonPort() > 0) { //DEBUG! REPLACE!
talons[currentTalonIndex]->restart(); //DEBUG! Do only if talon is associated with sensor, and object exists //DEBUG! REPLACE!
}
if(sensors[i]->getSensorPort() > 0 && sensors[i]->getTalonPort() > 0) { //If a Talon is associated with the sensor, turn that port on
talons[currentTalonIndex]->disableDataAll(); //Turn off all data on Talon
// talons[sensors[i]->getTalonPort() - 1]->enablePower(sensors[i]->getSensorPort(), true); //Turn on power for the given port on the Talon
talons[currentTalonIndex]->enableData(sensors[i]->getSensorPort(), true); //Turn back on only port used
}
}
if(sensors[i]->getTalonPort() == 0 && sensors[i]->sensorInterface != BusType::CORE) {
missingSensor = true; //Set flag if any sensors not assigned to Talon and not a core sensor
Serial.print("Missing Sensor: "); //DEBUG!
Serial.print(i);
Serial.print("\t");
Serial.println(sensors[i]->sensorInterface);
}
bool hasCriticalError = false;
bool hasError = false;
String val;
if(sensors[i]->getTalonPort() > 0 && sensors[i]->getSensorPort() > 0) val = sensors[i]->begin(logger.getTime(), hasCriticalError, hasError); //If detected sensor, run begin
else if(sensors[i]->getTalonPort() > 0 && sensors[i]->getSensorPort() == 0 || sensors[i]->sensorInterface == BusType::CORE) val = sensors[i]->selfDiagnostic(2, logger.getTime()); //If sensor is a Talon or CORE type, run diagnostic, begin has already been run
if(hasError) reportError = true; //Set if any of them throw an error
if(hasCriticalError) reportCriticalError = true; //Set if any of them throw a critical error
if(!val.equals("")) { //Only append if not empty string
if(output.length() - output.lastIndexOf('\n') + val.length() + closer.length() + 1 < Kestrel::MAX_MESSAGE_LENGTH) { //Add +1 to account for comma appending, subtract any previous lines from count
if(deviceCount > 0) output = output + ","; //Add preceeding comma if not the first entry
output = output + "{" + val + "}"; //Append result
deviceCount++;
// if(i + 1 < sensors.size()) diagnostic = diagnostic + ","; //Only append if not last entry
}
else {
output = output + closer + "\n"; //End this packet
output = output + leader + "{" + val + "}"; //Start a new packet and add new payload
}
}
}
if(missingSensor) logger.setIndicatorState(IndicatorLight::SENSORS, IndicatorMode::ERROR);
else logger.setIndicatorState(IndicatorLight::SENSORS, IndicatorMode::PASS); //If no errors are reported, set to pass state
//FIX! Replace!
// if(reportCriticalError) logger.setIndicatorState(IndicatorLight::SENSORS, IndicatorMode::ERROR_CRITICAL);
// else if(reportError) logger.setIndicatorState(IndicatorLight::SENSORS, IndicatorMode::ERROR); //Only set minimal error state if critical error is not thrown
// else logger.setIndicatorState(IndicatorLight::SENSORS, IndicatorMode::PASS); //If no errors are reported, set to pass state
output = output + closer; //Close diagnostic
return output;
}
void quickTalonShutdown()
{
// Wire.beginTransmission(0x22); //Talk to I2C Talon
// Wire.write(0x48); //Point to pullup/pulldown select reg
// Wire.write(0xF0); //Set pins 1 - 4 as pulldown
// Wire.endTransmission();
// Wire.beginTransmission(0x22); //Talk to I2C Talon
// Wire.write(0x46); //Point to pullup/pulldown enable reg
// Wire.write(0x0F); //Enable pulldown on pins 1-4
// Wire.endTransmission();
//////////// DEBUG! /////////////
//// SET SDI-12 TALON First to eliminiate issue with power being applied to Apogee port
Wire.beginTransmission(0x25); //Talk to SDI12 Talon
Wire.write(0x02); //Point to output port
Wire.write(0x00); //Set pints 1 - 8 low
Wire.endTransmission();
Wire.beginTransmission(0x25); //Talk to SDI12 Talon
Wire.write(0x06); //Point to config port
Wire.write(0x00); //Set pins 1 - 8 as output
Wire.endTransmission();
Wire.beginTransmission(0x25); //Talk to SDI12 Talon
Wire.write(0x00); //Point to port reg
// Wire.write(0xF0); //Set pints 1 - 4 low
Wire.endTransmission();