-
Notifications
You must be signed in to change notification settings - Fork 0
/
MultiWii.ino
2526 lines (2361 loc) · 77.8 KB
/
MultiWii.ino
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
/*
MultiWiiCopter by Alexandre Dubus
www.multiwii.com
March 2013 V2.2
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
any later version. see <http://www.gnu.org/licenses/>
*/
#include <avr/io.h>
#include "config.h"
#include "def.h"
#include <avr/pgmspace.h>
#define VERSION 221
/*********** RC alias *****************/
enum rc
{
ROLL,
PITCH,
YAW,
THROTTLE,
AUX1,
AUX2,
AUX3,
AUX4
};
enum alt_target_mode {
//0 - to failsafe RTH altitude ; 1 - to failsafe HOME altitude ; 2 - to RTH altitude ; 3 - to HOME altitude ; 4 - to WP altitude
alt_target_mode_failsafe_rth_altitude = 0,
alt_target_mode_failsafe_home_altitude,
alt_target_mode_rth_altitude,
alt_target_mode_home_altitude,
alt_target_mode_wp_altitude,
};
enum pid
{
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL, // not used currently
PIDITEMS
};
const char pidnames[] PROGMEM =
"ROLL;"
"PITCH;"
"YAW;"
"ALT;"
"Pos;"
"PosR;"
"NavR;"
"LEVEL;"
"MAG;"
"VEL;"
;
enum box
{
BOXARM,
#if ACC
BOXANGLE,
BOXHORIZON,
#endif
#if OPTFLOW
BOXOPTFLOW,
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
BOXBARO,
#endif
#ifdef VARIOMETER
BOXVARIO,
#endif
#if MAG
BOXMAG,
BOXHEADFREE,
BOXHEADADJ, // acquire heading for HEADFREE mode
#endif
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(SERVO_MIX_TILT)
BOXCAMSTAB,
#endif
#if defined(CAMTRIG)
BOXCAMTRIG,
#endif
#if GPS
BOXGPSHOME,
BOXGPSHOLD,
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
BOXPASSTHRU,
#endif
#if defined(BUZZER)
BOXBEEPERON,
#endif
#if defined(LED_FLASHER)
BOXLEDMAX, // we want maximum illumination
BOXLEDLOW, // low/no lights
#endif
#if defined(LANDING_LIGHTS_DDR)
BOXLLIGHTS, // enable landing lights at any altitude
#endif
#ifdef INFLIGHT_ACC_CALIBRATION
BOXCALIB,
#endif
#ifdef GOVERNOR_P
BOXGOV,
#endif
#ifdef OSD_SWITCH
BOXOSD,
#endif
#if defined(AUTOLAND) && BARO && GPS
BOXAUTOLAND,
#endif
CHECKBOXITEMS
};
const char boxnames[] PROGMEM = // names for dynamic generation of config GUI
"ARM;"
#if ACC
"ANGLE;"
"HORIZON;"
#endif
#if OPTFLOW
"FLOW;"
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
"BARO;"
#endif
#ifdef VARIOMETER
"VARIO;"
#endif
#if MAG
"MAG;"
"HEADFREE;"
"HEADADJ;"
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
"CAMSTAB;"
#endif
#if defined(CAMTRIG)
"CAMTRIG;"
#endif
#if GPS
"GPS HOME;"
"GPS HOLD;"
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
"PASSTHRU;"
#endif
#if defined(BUZZER)
"BEEPER;"
#endif
#if defined(LED_FLASHER)
"LEDMAX;"
"LEDLOW;"
#endif
#if defined(LANDING_LIGHTS_DDR)
"LLIGHTS;"
#endif
#ifdef INFLIGHT_ACC_CALIBRATION
"CALIB;"
#endif
#ifdef GOVERNOR_P
"GOVERNOR;"
#endif
#ifdef OSD_SWITCH
"OSD SW;"
#endif
#if defined(AUTOLAND) && BARO && GPS
"AUTOLAND;"
#endif
;
const uint8_t boxids[] PROGMEM = // permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function.
{
0, //"ARM;"
#if ACC
1, //"ANGLE;"
2, //"HORIZON;"
#endif
#if OPTFLOW
3, //BOXOPTFLOW,
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
4, //"BARO;"
#endif
#ifdef VARIOMETER
5, //"VARIO;"
#endif
#if MAG
6, //"MAG;"
7, //"HEADFREE;"
8, //"HEADADJ;"
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
9, //"CAMSTAB;"
#endif
#if defined(CAMTRIG)
10, //"CAMTRIG;"
#endif
#if GPS
11, //"GPS HOME;"
12, //"GPS HOLD;"
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
13, //"PASSTHRU;"
#endif
#if defined(BUZZER)
14, //"BEEPER;"
#endif
#if defined(LED_FLASHER)
15, //"LEDMAX;"
16, //"LEDLOW;"
#endif
#if defined(LANDING_LIGHTS_DDR)
17, //"LLIGHTS;"
#endif
#ifdef INFLIGHT_ACC_CALIBRATION
18, //"CALIB;"
#endif
#ifdef GOVERNOR_P
19, //"GOVERNOR;"
#endif
#ifdef OSD_SWITCH
20, //"OSD_SWITCH;"
#endif
#if defined(AUTOLAND) && BARO && GPS
21, //"AUTOLAND;"
#endif
};
static uint32_t currentTime = 0;
static uint16_t previousTime = 0;
static uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
static uint16_t calibratingA = 0; // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
static uint16_t calibratingG;
static uint16_t acc_1G; // this is the 1G measured acceleration
static uint16_t acc_25deg;
static uint8_t vbatMin = VBATNOMINAL; // lowest battery voltage in 0.1V steps
static uint8_t rcOptions[CHECKBOXITEMS];
static int32_t BaroAlt; // in cm
static int16_t BaroPID = 0;
static int16_t errorAltitudeI = 0;
static uint16_t failsafe_rcdata_throttle = 0;
static int16_t AltVarioCorr = 0;
static uint8_t AltVarioChanged = 1;
static int16_t initialThrottleHold; // placed here for separated baro-code
// **************
// gyro+acc IMU
// **************
static int16_t gyroZero[3] = {0, 0, 0};
static struct
{
int16_t accSmooth[3];
int16_t gyroData[3];
int16_t magADC[3];
int16_t gyroADC[3];
int16_t accADC[3];
} imu;
static struct
{
uint8_t vbat; // battery voltage in 0.1V steps
uint16_t intPowerMeterSum;
uint16_t rssi; // range: [0;1023]
} analog;
static struct
{
int32_t EstAlt; // in cm
int16_t vario; // variometer in cm/s
int32_t AltHold; // in mm
} alt;
static struct
{
int16_t angle[2];
int16_t heading;
int16_t headFreeModeHold;
int16_t magHold; // [-180;+180]
} att;
static struct
{
int16_t events;
uint8_t active; // Failsafe mode status - replace (failsafeCnt > (5*FAILSAFE_DELAY))
#if BARO && defined(FAILSAFE) && (defined(FAILSAFE_ALT_MODE) || defined(FAILSAFE_RTH_MODE))
uint8_t altSet;
#if defined(FAILSAFE_RTH_MODE)
uint8_t confSet;
uint8_t atHome;
uint32_t atHomeDelay;
#if defined(FAILSAFE_RTH_START_DELAY)
uint32_t startDelay;
#endif
#endif
#endif
} failsafe;
#if defined(ARMEDTIMEWARNING)
static uint32_t ArmedTimeWarningMicroSeconds = 0;
#endif
static int16_t debug[4];
struct flags_struct
{
uint8_t OK_TO_ARM : 1 ;
uint8_t ARMED : 1 ;
uint8_t I2C_INIT_DONE : 1 ; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup())
uint8_t ACC_CALIBRATED : 1 ;
uint8_t NUNCHUKDATA : 1 ;
uint8_t ANGLE_MODE : 1 ;
uint8_t HORIZON_MODE : 1 ;
uint8_t MAG_MODE : 1 ;
uint8_t BARO_MODE : 1 ;
uint8_t GPS_HOME_MODE : 1 ;
uint8_t GPS_HOLD_MODE : 1 ;
uint8_t HEADFREE_MODE : 1 ;
uint8_t PASSTHRU_MODE : 1 ;
uint8_t GPS_FIX : 1 ;
uint8_t GPS_FIX_HOME : 1 ;
uint8_t SMALL_ANGLES_25 : 1 ;
uint8_t CALIBRATE_MAG : 1 ;
uint8_t VARIO_MODE : 1;
uint8_t AUTOLAND_MODE : 1;
#if defined(OPTFLOW)
uint8_t OPTFLOW_MODE: 1;
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
uint8_t ACC_INFLIGHT_CALI_ARMED:1;
uint8_t ACC_INFLIGHT_CALI_MEASUREMENT_DONE:1;
uint8_t ACC_INFLIGHT_CALI_SAVE:1;
uint8_t ACC_INFLIGHT_CALI_ACTIVE:1;
#endif
} f;
//for log
#if defined(LOG_VALUES) || defined(LCD_TELEMETRY)
static uint16_t cycleTimeMax = 0; // highest ever cycle timen
static uint16_t cycleTimeMin = 65535; // lowest ever cycle timen
static uint16_t powerMax = 0; // highest ever current;
static int32_t BAROaltMax; // maximum value
#endif
#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
static uint32_t armedTime = 0;
#endif
static int16_t i2c_errors_count = 0;
static int16_t annex650_overrun_count = 0;
#if defined(THROTTLE_ANGLE_CORRECTION)
static int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind, //NHADRIAN
#endif
static int8_t cosZ = 100; // cos(angleZ)*100
// **********************
//Automatic ACC Offset Calibration
// **********************
#if defined(INFLIGHT_ACC_CALIBRATION)
static int8_t InflightcalibratingA = 0;
#endif
// **********************
// power meter
// **********************
#if defined(POWERMETER)
#define PMOTOR_SUM 8 // index into pMeter[] for sum
static uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum
static uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
static uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
static uint16_t powerValue = 0; // last known current
#endif
static uint16_t intPowerTrigger1;
// **********************
// telemetry
// **********************
#if defined(LCD_TELEMETRY)
static uint8_t telemetry = 0;
static uint8_t telemetry_auto = 0;
#endif
#ifdef LCD_TELEMETRY_STEP
static char telemetryStepSequence [] = LCD_TELEMETRY_STEP;
static uint8_t telemetryStepIndex = 0;
#endif
// ******************
// rc functions
// ******************
#define MINCHECK 1100
#define MAXCHECK 1900
#define ROL_LO (1<<(2*ROLL))
#define ROL_CE (3<<(2*ROLL))
#define ROL_HI (2<<(2*ROLL))
#define PIT_LO (1<<(2*PITCH))
#define PIT_CE (3<<(2*PITCH))
#define PIT_HI (2<<(2*PITCH))
#define YAW_LO (1<<(2*YAW))
#define YAW_CE (3<<(2*YAW))
#define YAW_HI (2<<(2*YAW))
#define THR_LO (1<<(2*THROTTLE))
#define THR_CE (3<<(2*THROTTLE))
#define THR_HI (2<<(2*THROTTLE))
volatile int16_t failsafeCnt = 0;
static int32_t failsafe_begin_alt = 0;
static uint8_t failsafeLowBaroPIDs = 0;
static uint8_t failsafeLowSonarAlt = 0;
#ifdef RCSERIAL
static int16_t rcData[RC_CHANS] = { 1500, 1500, 1500, 1000, 1000, 1000, 1000, 1000 }; // interval [1000;2000]
#else
static int16_t rcData[RC_CHANS]; // interval [1000;2000]
#endif
static int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
static int16_t lookupPitchRollRC[6];// lookup table for expo & RC rate PITCH+ROLL
static int16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE
#if defined(SPEKTRUM)
volatile uint8_t spekFrameFlags;
volatile uint32_t spekTimeLast;
#endif
#if defined(OPENLRSv2MULTI)
static uint8_t pot_P, pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages
#endif
// *************************
// motor and servo functions
// *************************
static int16_t axisPID[3];
static int16_t motor[8];
static int16_t servo[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
// ************************
// EEPROM Layout definition
// ************************
static uint8_t dynP8[3], dynD8[3];
static struct
{
uint8_t currentSet;
int16_t accZero[3];
int16_t magZero[3];
uint8_t checksum; // MUST BE ON LAST POSITION OF STRUCTURE !
} global_conf;
typedef struct pid_
{
uint8_t P8;
uint8_t I8;
uint8_t D8;
};
static struct
{
uint8_t checkNewConf;
pid_ pid[PIDITEMS];
uint8_t rcRate8;
uint8_t rcExpo8;
uint8_t rollPitchRate;
uint8_t yawRate;
uint8_t dynThrPID;
uint8_t thrMid8;
uint8_t thrExpo8;
int16_t angleTrim[2];
uint16_t activate[CHECKBOXITEMS];
uint8_t powerTrigger1;
#ifdef FLYING_WING
uint16_t wing_left_mid;
uint16_t wing_right_mid;
#endif
#ifdef TRI
uint16_t tri_yaw_middle;
#endif
#if defined HELICOPTER || defined(AIRPLANE)|| defined(SINGLECOPTER)|| defined(DUALCOPTER)
int16_t servoTrim[8];
#endif
#if defined(GYRO_SMOOTHING)
uint8_t Smoothing[3];
#endif
#if defined (FAILSAFE)
int16_t failsafe_throttle;
#endif
#ifdef VBAT
uint8_t vbatscale;
uint8_t vbatlevel_warn1;
uint8_t vbatlevel_warn2;
uint8_t vbatlevel_crit;
uint8_t no_vbat;
#endif
#ifdef POWERMETER
uint16_t psensornull;
uint16_t pleveldivsoft;
uint16_t pleveldiv;
uint8_t pint2ma;
#endif
#ifdef CYCLETIME_FIXATED
uint16_t cycletime_fixated;
#endif
#ifdef MMGYRO
uint8_t mmgyro;
#endif
#ifdef ARMEDTIMEWARNING
uint16_t armedtimewarning;
#endif
int16_t minthrottle;
#ifdef GOVERNOR_P
int16_t governorP;
int16_t governorD;
int8_t governorR;
#endif
uint8_t checksum; // MUST BE ON LAST POSITION OF CONF STRUCTURE !
} conf;
#ifdef LOG_PERMANENT
static struct
{
uint16_t arm; // #arm events
uint16_t disarm; // #disarm events
uint16_t start; // #powercycle/reset/initialize events
uint32_t armed_time ; // copy of armedTime @ disarm
uint32_t lifetime; // sum (armed) lifetime in seconds
uint16_t failsafe; // #failsafe state @ disarm
uint16_t i2c; // #i2c errs state @ disarm
uint8_t running; // toggle on arm & disarm to monitor for clean shutdown vs. powercut
uint8_t checksum; // MUST BE ON LAST POSITION OF CONF STRUCTURE !
} plog;
#endif
// **********************
// GPS common variables
// **********************
static int16_t GPS_angle[2] = { 0, 0}; // the angles that must be applied for GPS correction
static int32_t GPS_coord[2];
static uint8_t GPS_numSat;
static uint16_t GPS_distanceToHome; // distance to home - unit: meter
static int16_t GPS_directionToHome; // direction to home - unit: degree
static uint16_t GPS_altitude; // GPS altitude - unit: meter
static uint16_t GPS_speed; // GPS speed - unit: cm/s
static uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update
static uint16_t GPS_ground_course = 0; // - unit: degree*10
static uint8_t GPS_Present = 0; // Checksum from Gps serial
static uint8_t GPS_Enable = 0;
#define LAT 0
#define LON 1
// The desired bank towards North (Positive) or South (Negative) : latitude
// The desired bank towards East (Positive) or West (Negative) : longitude
static int16_t nav[2];
static int16_t nav_rated[2]; //Adding a rate controller to the navigation to make it smoother
/* Exponential moving average filter (optimized for integers) with factor = 2^n */
typedef struct avg_var16
{
int32_t buf; // internal bufer to store non-rounded average value
int16_t res; // result (rounded to int)
} t_avg_var16;
typedef struct avg_var8
{
int16_t buf; // internal bufer to store non-rounded average value
int8_t res; // result (rounded to int)
} t_avg_var8;
#if SONAR
static int16_t SonarAlt = 0; // distance, cm (0..SONAR_MAX_DISTANCE)
static uint8_t SonarErrors = 0; // errors count (0..SONAR_ERROR_MAX).
static uint32_t SonarTimer = 0;
static t_avg_var16 baroSonarDiff = {0, 0};
#endif
#if defined(OPTFLOW) || defined(I2C_OPTFLOW)
/* Angles of correction */
static int16_t optflow_angle[2] = { 0, 0 };
static uint8_t optflow_paused;
#endif
// default POSHOLD control gains
#define POSHOLD_P .11
#define POSHOLD_I 0.0
#define POSHOLD_IMAX 20 // degrees
#define POSHOLD_RATE_P 2.0
#define POSHOLD_RATE_I 0.08 // Wind control
#define POSHOLD_RATE_D 0.045 // try 2 or 3 for POSHOLD_RATE 1
#define POSHOLD_RATE_IMAX 20 // degrees
// default Navigation PID gains
#define NAV_P 1.4
#define NAV_I 0.20 // Wind control
#define NAV_D 0.08 //
#define NAV_IMAX 20 // degrees
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Serial GPS only variables
//navigation mode
#define NAV_MODE_NONE 0
#define NAV_MODE_POSHOLD 1
#define NAV_MODE_WP 2
static uint8_t nav_mode = NAV_MODE_NONE; // Navigation mode
static uint8_t alarmArray[16]; // array
/*** GPS WP datas***/
// suggested naw flags:
#define HOME 0
#define HOLD 1
// 2-WP_NUMBER - generic WP navigation */
static struct
{
int32_t Lat; // Lat for WP
int32_t Lon; // Lon for WP
int32_t Alt; // alt at WP (cm)
int16_t Heading; // heading at WP (deg)
uint8_t Vario; // vario for alt changing (cm/s) - maximum 250
uint8_t Time; // time to stay in WP (s) - maximum 250
uint8_t Updated; // WP is updated or not or not?
// 0 - current WP is not yet updated
// 1 - current WP is already updated
} WP[WP_NUMBER];
#if BARO
static int32_t baroPressure;
static int32_t baroTemperature;
static int32_t baroPressureSum;
#if !defined(SUPPRESS_BARO_ALTHOLD) || (defined(FAILSAFE) && (defined(FAILSAFE_ALT_MODE) || defined(FAILSAFE_RTH_MODE)))
static uint8_t isAltHoldChanged = 1;
static int16_t targetVario = 0;
#endif
#if defined(AUTOLAND) && defined(RTH_ALT_MODE)
static uint8_t autolandAltSet = 0;
#endif
#if (defined(VARIO_ALT_MODE) || defined(RTH_ALT_MODE) || defined(WP_ALT_MODE)) && !defined(SUPPRESS_BARO_ALTHOLD) || (defined(FAILSAFE) && (defined (FAILSAFE_ALT_MODE) || defined(FAILSAFE_RTH_MODE)))
static int16_t AltVario = 0;
static uint8_t AltHoldRising = 1;
static uint8_t targetAltReached;
/* 0 - Target Alt not reached
1 - Target Alt reached */
#endif
#endif
#if defined (SERVO_TILT_NHADRIAN)
static struct
{
int16_t dif[2];
int16_t prev[2];
int16_t velPrev[2];
int16_t acc[2];
} camstab;
#endif
void annexCode() // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
{
static uint32_t calibratedAccTime;
uint16_t tmp, tmp2;
uint8_t axis, prop1, prop2;
#define BREAKPOINT 1500
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < BREAKPOINT)
{
prop2 = 100;
}
else
{
if (rcData[THROTTLE] < 2000)
{
prop2 = 100 - (uint16_t)conf.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
}
else
{
prop2 = 100 - conf.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++)
{
tmp = min(abs(rcData[axis] - MIDRC), 500);
#if defined(DEADBAND)
if (tmp > DEADBAND)
{
tmp -= DEADBAND;
}
else
{
tmp = 0;
}
#endif
if (axis != 2) //ROLL & PITCH
{
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)conf.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
}
else // YAW
{
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t)conf.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t)conf.pid[axis].P8 * prop1 / 100;
dynD8[axis] = (uint16_t)conf.pid[axis].D8 * prop1 / 100;
if (rcData[axis] < MIDRC) rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], MINCHECK, 2000);
tmp = (uint32_t)(tmp - MINCHECK) * 1000 / (2000 - MINCHECK); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [conf.minthrottle;MAXTHROTTLE]
if (f.HEADFREE_MODE) //to optimize
{
float radDiff = (att.heading - att.headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
float cosDiff = cos(radDiff);
float sinDiff = sin(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
#if defined(POWERMETER_HARD)
uint16_t pMeterRaw; // used for current reading
static uint16_t psensorTimer = 0;
if (! (++psensorTimer % PSENSORFREQ))
{
pMeterRaw = analogRead(PSENSORPIN);
//lcdprint_int16(pMeterRaw); LCDcrlf();
//debug[0] = pMeterRaw;
powerValue = ( conf.psensornull > pMeterRaw ? conf.psensornull - pMeterRaw : pMeterRaw - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun
if ( powerValue < 333) // only accept reasonable values. 333 is empirical
{
#ifdef LCD_TELEMETRY
if (powerValue > powerMax) powerMax = powerValue;
#endif
}
else
{
powerValue = 333;
}
pMeter[PMOTOR_SUM] += (uint32_t) powerValue;
}
#endif
#if defined(BUZZER)
#if defined(VBAT)
static uint8_t vbatTimer = 0;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
if (! (++vbatTimer % VBATFREQ))
{
vbatRawArray[(ind++) % 8] = analogRead(V_BATPIN);
for (uint8_t i = 0; i < 8; i++) vbatRaw += vbatRawArray[i];
analog.vbat = (vbatRaw * 2) / conf.vbatscale; // result is Vbatt in 0.1V steps
}
#endif
alarmHandler(); // external buzzer routine that handles buzzer events globally now
#endif
#if defined(RX_RSSI)
static uint8_t sig = 0;
uint16_t rssiRaw = 0;
static uint16_t rssiRawArray[8];
rssiRawArray[(sig++) % 8] = analogRead(RX_RSSI_PIN);
for (uint8_t i = 0; i < 8; i++) rssiRaw += rssiRawArray[i];
analog.rssi = rssiRaw / 8;
#endif
if ( (calibratingA > 0 && ACC ) || (calibratingG > 0) ) // Calibration phasis
{
LEDPIN_TOGGLE;
}
else
{
if (f.ACC_CALIBRATED)
{
LEDPIN_OFF;
}
if (f.ARMED)
{
LEDPIN_ON;
}
}
#if defined(LED_RING)
static uint32_t LEDTime;
if ( currentTime > LEDTime )
{
LEDTime = currentTime + 50000;
i2CLedRingState();
}
#endif
#if defined(LED_FLASHER)
auto_switch_led_flasher();
#endif
if ( currentTime > calibratedAccTime )
{
if (! f.SMALL_ANGLES_25)
{
// the multi uses ACC and is not calibrated or is too much inclinated
f.ACC_CALIBRATED = 0;
LEDPIN_TOGGLE;
calibratedAccTime = currentTime + 100000;
}
else
{
f.ACC_CALIBRATED = 1;
}
}
#if !(defined(SPEKTRUM) && defined(PROMINI)) //Only one serial port on ProMini. Skip serial com if Spektrum Sat in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0.
#if defined(GPS_PROMINI)
if (GPS_Enable == 0)
{
serialCom();
}
#else
serialCom();
#endif
#endif
#if defined(POWERMETER)
analog.intPowerMeterSum = (pMeter[PMOTOR_SUM] / conf.pleveldiv);
intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE;
#endif
#ifdef LCD_TELEMETRY_AUTO
static char telemetryAutoSequence [] = LCD_TELEMETRY_AUTO;
static uint8_t telemetryAutoIndex = 0;
static uint16_t telemetryAutoTimer = 0;
if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) ) )
{
telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)];
LCDclear(); // make sure to clear away remnants
}
#endif
#ifdef LCD_TELEMETRY
static uint16_t telemetryTimer = 0;
if (! (++telemetryTimer % LCD_TELEMETRY_FREQ))
{
#if (LCD_TELEMETRY_DEBUG+0 > 0)
telemetry = LCD_TELEMETRY_DEBUG;
#endif
if (telemetry) lcd_telemetry();
}
#endif
#if GPS & defined(GPS_LED_INDICATOR) // modified by MIS to use STABLEPIN LED for number of sattelites indication
static uint32_t GPSLEDTime; // - No GPS FIX -> LED blink at speed of incoming GPS frames
static uint8_t blcnt; // - Fix and sat no. bellow 5 -> LED off
if (currentTime > GPSLEDTime) // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ...
{
if (f.GPS_FIX && GPS_numSat >= 5)
{
if (++blcnt > 2 * GPS_numSat) blcnt = 0;
GPSLEDTime = currentTime + 150000;
if (blcnt >= 10 && ((blcnt % 2) == 0))
{
STABLEPIN_ON;
}
else
{
STABLEPIN_OFF;
}
}
else
{
if ((GPS_update == 1) && !f.GPS_FIX)
{
STABLEPIN_ON;
}
else
{
STABLEPIN_OFF;
}
blcnt = 0;
}
}
#endif
#if defined(LOG_VALUES) && (LOG_VALUES >= 2)
if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore
if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore
#endif
if (f.ARMED)
{
#if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
armedTime += (uint32_t)cycleTime;
#endif
#if defined(VBAT)
if ( (analog.vbat > conf.no_vbat) && (analog.vbat < vbatMin) ) vbatMin = analog.vbat;
#endif
#ifdef LCD_TELEMETRY
#if BARO
if ( (BaroAlt > BAROaltMax) ) BAROaltMax = BaroAlt;
#endif
#endif
}
}
void setup()
{
#if defined(INFLIGHT_ACC_CALIBRATION)
f.ACC_INFLIGHT_CALI_ARMED = 0;
f.ACC_INFLIGHT_CALI_MEASUREMENT_DONE = 0;
f.ACC_INFLIGHT_CALI_SAVE = 0;
f.ACC_INFLIGHT_CALI_ACTIVE = 0;
#endif
#if !defined(GPS_PROMINI)
SerialOpen(0, SERIAL0_COM_SPEED);
#if defined(PROMICRO)
SerialOpen(1, SERIAL1_COM_SPEED);
#endif
#if defined(MEGA)
SerialOpen(1, SERIAL1_COM_SPEED);
SerialOpen(2, SERIAL2_COM_SPEED);
SerialOpen(3, SERIAL3_COM_SPEED);
#endif
#endif
LEDPIN_PINMODE;
POWERPIN_PINMODE;
BUZZERPIN_PINMODE;
STABLEPIN_PINMODE;
POWERPIN_OFF;
initOutput();
#ifdef MULTIPLE_CONFIGURATION_PROFILES
for (global_conf.currentSet = 0; global_conf.currentSet < 3; global_conf.currentSet++) // check all settings integrity
{
readEEPROM();
}
#else
global_conf.currentSet = 0;
readEEPROM();
#endif
readGlobalSet();
readEEPROM(); // load current setting data
blinkLED(2, 40, global_conf.currentSet + 1);
configureReceiver();
#if defined (PILOTLAMP)
PL_INIT;
#endif
#if defined(OPENLRSv2MULTI)
initOpenLRS();
#endif
initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
GPS_set_pids();
#endif
previousTime = micros();
#if defined(GIMBAL)
calibratingA = 512;
#endif
calibratingG = 512;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER)
for (uint8_t i = 0; i <= PMOTOR_SUM; i++)
pMeter[i] = 0;
#endif
/************************************/
#if defined(GPS_SERIAL)
GPS_SerialInit();
for (uint8_t i = 0; i <= 5; i++)
{
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
if (!GPS_Present)
{
SerialEnd(GPS_SERIAL);